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内 容 简 介 


本 书 系统 地 介绍 了 机 械 手 控制 的 几 种 先进 设计 方法 ,是 作者 多 年 来 从 事 机 器 人 控制 系统 教学 和 科研 
工作 的 结晶 ,同时 融和 人 了 国内 外 同行 近年 来 所 取得 的 最 新 成 果 。 

本 书 主要 以 机 械 手 的 控制 为 论述 对 象 , 共 包 括 16 章 内 容 , 分 别 介绍 机 械 手 PD 控制 ,神经 网 络 自 适应 
控制 .模糊 自 适应 控制 .和 迭代 学 习 控制 . 反 演 控制 、. 滑 模 控 制 . 和 白 适 应 鲁 棒 控 制 . 未 端 轨 迹 及 力 的 连续 切换 滑 
模 控 制 .重复 控制 的 基本 原理 及 设计 方法 .机 械 手 容错 控制 .基于 事件 驱动 的 机 械 手 反 演 控制 .基于 输入 延 
迟 的 机 械 手 控制 .基于 执行 器 量化 的 控制 ,基于 控制 方向 未 知 的 控制 和 多 智能 体系 统一 致 性 控制 的 设计 与 
分 析 。 每 种 方法 都 给 出 了 算法 推导 、 实 例 分 析 和 相应 的 MATLAB 仿真 设计 程序 。 

本 书 各 部 分 内 容 既 相互 联系 又 相互 独立 ,读者 可 根据 自己 的 需要 选择 学 习 。 本 书 适合 从 事 生 产 过 程 
自动 化 .计算 机 应 用 、 机 械 电 子 和 电气 自动 化 领域 工作 的 工程 技术 人 员 阅 读 , 也 可 作为 高 等 院 校 工 业 自 动 
化 .自动 控制 .机 械 电子 .自动 化 仪表 .计算 机 应 用 等 专业 的 教学 参考 书 。 
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1| 作 | 者 | 简 | 介 | 


刘 金 琨 ”北京 航空 航天 大 学 教授 ， 博 士 
生 导 师 。 分 别 于 1989 年 7 月 、1994 年 3 月 和 
1997 年 3 月 获 东北 大 学 工学 学 士 、 工 学 硕士 和 
工学 博士 学 位 。1997 年 3 月 一 1998 年 12 月 在 
浙江 大 学 工业 控制 技术 研究 所 做 博士 后 研究 工 
作 ; 1999 年 1 月 一 1999 年 7 月 在 香港 科技 大 
学 从 事 合作 研究 ; 1999 年 11 月 至 今 在 北京 航 
空 航天 大 学 自动 化 科学 与 电气 工程 学 院 从 事 教 
学 与 科研 工作 ， 主 讲 “ 智 能 控制 ”“ 先 进 控制 
系统 设计 ”“ 系 统 辨识 ”等 课程 ， 主 要 研究 方 
向 为 控制 理论 与 应 用 。 先 后 主持 国家 自然 科学 
基金 等 科研 项 目 10 余 项 ， 发 表 学 术 论文 100 余 
篇 ， 出 版 《先进 PID 控 制 MATLAB 仿 真 》《 滑 
模 变 结构 控制 MATLAB 仿 真 》《 机 器 人 控制 
系统 的 设计 与 MATLAB 仿 真 》《 RBF 神经 网 
络 自 适 应 控制 MATLAB 仿 真 》《 系统 辨识 
和 《智能 控制 一 一 理论 基础 、 算 法 设计 与 应 
用 》 等 著作 。 


| 学 | 习 | 资 | 源 


为 便于 读者 学 习 与 动手 实践 ， 本 书 配套 提 
供 程序 代码 ， 关 注 “ 人 工 智 能 科学 与 技术 ” 微 
信 公 众 号 ， 在 “知识 ”一 “资源 下 载 ” 一 “ 配 
书 资源 ”菜单 获取 下 载 链接 ( 或 到 清华 大 学 出 
版 社 网 站 本 书页 面 获 取 下 载 链接 ) 。 
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PREFACE 


有 关机 器 人 控制 理论 及 其 工程 应 用 ,近年 来 已 有 大 量 的 论文 发 表 。 作 者 多 年 来 一 直 从 
事 控制 理论 及 应 用 方面 的 教学 和 研究 工作 ,为 了 促进 机 器 人 控制 和 自动 化 技术 的 进步 ,反映 
机 器 人 控制 设计 与 应 用 中 的 最 新 研究 成 果 ,并 使 广大 研究 人 员 和 工程 技术 人 员 能 了 解 、. 掌 握 
和 应 用 这 一 领域 的 最 新 技术 ,学 会 用 MATLAB 语言 进行 各 种 机 器 人 控制 算法 的 分 析 和 设 
计 , 作 者 编写 了 本 书 , 以 抛砖引玉 , 供 广 大 读者 学 习 参 考 。 

本 书 是 在 总 结 作者 多 年 研究 成 果 的 基础 上 ,进一步 理论 化 .系统 化 规范化、 实用 化 而 成 
的 ,特点 如 下 : 

(1) 控制 算法 取材 新 颖 ,内 容 先进 ,重点 置 于 学 科 交叉 部 分 的 前 沿 研究 和 介绍 一 些 有 淤 
力 的 新 思想 .新 方法 和 新 技术 ,取材 着 重 于 基本 概念 、 基 本 理论 和 基本 方法 。 

(2) 针对 每 种 控制 算法 给 出 了 完整 的 MATLAB 仿真 程序 ,并 给 出 了 程序 的 说 明和 仿 
真 结果 ,具有 很 强 的 可 读 性 。 

(3) 着 重 从 应 用 角度 出 发 ,突出 理论 联系 实际 ,面向 广大 工程 技术 人 员 , 具 有 很 强 的 工 
程 性 和 实用 性 。 书 中 有 大 量 应 用 实例 及 结果 分 析 ,为 读者 提供 了 有 益 的 借鉴 。 

(4) 所 给 出 的 各 种 控制 算法 完整 ,结构 设计 力求 简单 明了 ,便于 自学 和 进一步 开发 。 

(5) 所 介绍 的 方法 不 局 限于 机 械 手 的 控制 ,同时 也 适合 解决 运动 控制 领域 其 他 背景 的 
控制 问题 。 

本 书 主要 以 机 器 人 力 臂 为 被 控 对 象 , 此 外 ,为 了 介绍 一 些 新 的 运动 控制 方法 ,本 书 还 以 
机 械 系 统 `. 电 机、 倒立 摆 为 被 控 对 象 辅助 说 明 。 

本 书 是 在 原 有 《机 器 人 控制 系统 的 设计 与 MATLAB 仿真 ) 基 础 上 撰写 而 成 的 ,并 增加 、 
修改 和 删除 了 部 分 内 容 。 全 书 共 包括 16 章 内 容 。 第 1 章 为 绪论 ,介绍 机 器 人 的 几 种 控制 方 
法 及 模型 特性 ; 第 2 章 介绍 机 械 手 PD 控制 的 几 种 基本 设计 方法 ,通过 仿真 和 分 析 进 行 了 说 
明 ; 第 3 章 介绍 机 械 手 神经 网 络 自 适 应 控制 的 几 种 设计 方法 ; 第 4 章 介绍 基于 LMI 的 模糊 
鲁 棒 控 制 方法 和 几 种 机 械 手 模糊 自 适应 控制 器 的 设计 方法 ; 第 5 章 介绍 机 械 手 迭 代 学 习 控 
制 设计 方法 ; 第 6 章 介 绍 机 械 手 反 演 控制 的 设计 方法 ; 第 7 章 介 绍 机 械 手 滑 模 控制 基本 设 
计 方 法 ; 第 8 章 介绍 机 械 手 自 适应 鲁 棒 控制 方法 ,包括 鲁 棒 控制 器 和 自 适应 控制 器 的 设计 ; 
第 9 章 介 绍 机 械 手 末端 轨迹 及 力 控制 设计 方法 ; 第 10 章 介绍 重复 控制 的 基本 原理 及 设计 
方法 ; 第 11 章 介绍 机 械 手 容错 控制 器 的 设计 和 分 析 方 法 ; 第 12 章 介绍 基于 事件 驱动 的 机 
械 手 反 演 控制 设计 方法 ; 第 13 章 介绍 在 带 有 输入 延迟 的 输入 受 限 控制 下 的 机 械 手 控制 器 
设计 和 分 析 方 法 ; 第 14 章 介绍 基于 随机 量化 的 执行 器 量化 控制 器 设计 和 分 析 方 法 ; 第 15 
章 介 绍 基 于 控制 方向 未 知 的 反 演 控制 器 设计 和 分 析 方 法 ; 第 16 章 介 绍 多 智能 体系 统一 致 
性 控制 的 设计 与 分 析 。 
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本 书 介 绍 的 控制 方法 有 些 选 自 高 水 平 国 际 期 刊 中 的 经 典 控 制 方 法 ,并 对 其 中 的 一 些 算 
法 进行 了 修正 或 补充 。 通 过 对 一 些 典 型 控制 器 设计 方法 进行 详细 的 理论 分 析 和 仿真 分 析 ， 
使 一 些 深奥 的 控制 理论 易于 掌握 ,为 读者 的 深入 研究 打下 基础 。 

本 书 是 基于 英文 版 MATLAB 环境 下 开发 ( 书 中 仿真 图 的 图 字 均 为 英文 ) 。 书 中 各 章节 
的 内 容 具 有 很 强 的 独立 性 ,读者 可 以 结合 自己 的 方向 深入 地 进行 研究 。 

作者 在 研究 过 程 中 ,东北 大 学 徐 心 和 教授 和 薛 定 宇 教授 在 机 器 人 控制 及 仿真 等 方面 给 
予 作 者 很 多 的 指点 ,北京 航空 航天 大 学 尔 联 洁 教授 在 控制 理论 方面 给 予 作 者 多 年 的 指导 ,在 
此 深 表 感谢 。 

由 于 作者 水 平 有 限 , 书 中 难免 存在 一 些 不 足 之 处 ,真诚 欢迎 广大 读者 批评 指正 。 


X| & 5t 
2022 ^F. 1 月 
于 北京 航空 航天 大 学 


仿真 程序 使 用 说 明 


(1) 所 有 仿真 算法 程序 按 章 归 类 ,程序 名 与 书 中 一 一 对 应 。 

(2) 将 下 载 的 仿真 程序 复制 到 硬盘 中 MATLAB 运行 的 路 径 中 , 便 可 仿真 运行 。 

(3) 本 书 算法 程序 在 当前 MATLAB 版 本 下 运行 成 功 ,并 适用 于 其 他 更 高 级 版 本 。 
(4) 程序 下 载 : 到 清华 大 学 出 版 社 网 站 (www. tup. tsinghua. edu. cn) 本 书页 面 下 载 。 
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CHAPTER 1 


1.1 机 器 人 控制 方法 简介 


机 器 人 学 科 是 一 门 迅速 发 展 的 综合 性 前 沿 学 科 , 受 到 工业 界 和 学 术 界 的 高 度 重视 。 
机 器 人 的 核心 是 机 器 人 控制 系统 ,从 控制 工程 的 角度 来 看 ,机 器 人 是 一 个 非 线性 和 不 确定 性 
系统 ,机 器 人 智能 控制 是 近年 来 机 器 人 控制 领域 研究 的 前 沿 课题 ,已 取得 了 相当 丰富 的 
成 果 。 

机 器 人 轨迹 跟踪 控制 系统 的 主要 目的 是 通过 给 定 各 关节 的 驱动 力矩 ,使 得 机 器 人 的 位 
置 .速度 等 状态 变量 跟踪 给 定 的 理想 轨迹 。 


1.1.1 机 器 人 常用 的 控制 方法 


常用 的 机 器 人 控制 方法 有 以 下 几 种 。 

(D 基于 模型 的 控制 方法 : 与 一 般 的 机 械 系 统一 样 , 当 机 器 人 的 结构 及 其 机 械 参 数 确 
定 后 ,其 动态 特性 将 由 动力 学 方程 即 数学 模型 来 描述 。 因 此 ,可 以 采用 自动 控制 理论 所 提供 
的 设计 方法 ,通过 基于 数学 模型 的 方法 设计 机 器 人 控制 器 。 基 于 被 控 对 象 数 学 模型 的 控制 
方法 有 前 馈 补偿 控制 ,计算 力矩 法 、 最 优 控 制 方法 、 非 线性 反馈 控制 方法 等 。 但 在 实际 工程 
中 ,由 于 机 器 人 是 一 个 非 线性 和 不 确定 性 系统 ,很 难得 到 精确 的 机 器 人 数学 模型 ,使 这 些 方 
法 很 难得 到 实际 应 用 。 

(2) PID 控制 : 机 器 人 控制 常 采用 PD 控制 和 PID 控制 ,其 优点 是 控制 律 简单 ,易于 实 
现 , 无 须 建 模 ,但 这 类 方法 有 两 个 明显 的 缺点 ,一 是 难以 保证 受 控 机 器 人 具有 良好 的 动态 和 
静态 品质 ; 二 是 需要 较 大 的 控制 能 量 。 

G) 自 适应 控制 : 自 适应 控制 是 根据 要 求 的 性 能 指标 与 实际 系统 的 性 能 指标 相 比 较 所 
获得 的 信息 来 修正 控制 规律 或 控制 器 参数 ,使 系统 能 够 保持 最 优 或 次 最 优 工 作 状 态 的 控制 
方法 。 具 体 地 讲 ,就 是 控制 器 能 够 及 时 修正 自己 的 特性 以 适应 控制 对 象 和 外 部 扰动 的 动态 
特性 变化 ,使 整个 控制 系统 始终 获得 满意 的 性 能 ,其 缺点 是 在 线 辨识 参数 所 需 的 庞大 计算 ， 
对 实时 性 要 求 严 格 、 实 现 比 较 复杂 ,特别 是 存在 非 参数 不 确定 性 时 , 自 适 应 控制 难以 保证 系 
统 稳定 和 达到 一 定 的 控制 性 能 指标 。 

(4) 鲁 棒 控 制 : 是 一 种 保证 不 确定 系统 的 稳定 性 以 及 达到 满意 控制 效果 的 控制 方法 。 
鲁 棒 控 制 器 的 设计 仅 需 知道 限制 不 确定 性 的 最 大 可 能 值 的 边界 即 可 ,和 鲁 棒 控 制 可 同时 补偿 
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结构 和 非 结 构 不 确定 性 的 影响 ,这 也 正 是 鲁 棒 控制 优 于 自 适 应 控制 之 处 。 除 此 之 外 ,与 自 适 
应 控制 方法 相 比 ,和 鲁 棒 控 制 还 有 实现 简单 (没有 自 适 应 律 ) ,对 时 变 参数 以 及 非 结 构 非 线性 不 
确定 性 的 影响 有 更 好 的 补偿 效果 ,更 易于 保证 稳定 性 等 优点 。 

(5) 神经 网 络 控制 和 模糊 控制 : 神经 网 络 和 模糊 系统 具有 高 度 的 非 线性 逼近 映射 能 
力 , 神 经 网 络 和 模糊 系统 技术 的 发 展 为 解决 复杂 的 非 线 性 .不 确定 及 不 确 知 系统 的 控制 开辟 
了 新 途径 。 采 用 神经 网 络 和 模糊 系统 ,可 实现 对 机 器 人 动力 学 方程 中 未 知 部 分 的 在 线 精确 
通 近 ,从 而 可 通过 在 线 建 模 和 前 人 乌 补 偿 ,实现 机 器 人 的 高 精度 跟踪 。 

(6) 迭代 学 习 控制 : 是 智能 控制 中 具有 严格 数学 描述 的 一 个 分 支 , 适 合 于 解决 强 非 线 
性 、 强 耦合 、 建 模 难 .运动 具有 重复 性 的 对 象 的 高 精度 控制 问题 。 迭 代 学 习 控 制 方法 不 依赖 
于 系统 的 精确 数学 模型 ,算法 简单 。 与 鲁 棒 控 制 一 样 ,迭代 学 习 控 制 也 能 处 理 实际 系统 中 的 
不 确定 性 :但 它 能 实现 完全 跟踪 ,控制 器 形式 更 为 简单 且 需 要 较 少 的 先 验 知识 。 机 器 人 轨迹 
跟踪 控制 是 迭代 学 习 控制 应 用 的 典型 代表 。 

(7) 变 结构 控制 : 其 本 质 上 是 一 类 特殊 的 非 线 性 控制 ,其 非 线 性 表现 为 控制 的 不 连续 
性 。 由 于 滑动 模 态 可 以 进行 设计 且 与 对 象 参数 及 扰动 无 关 , 这 就 使 得 变 结构 控制 具有 快速 
响应 \ 对 参数 变化 及 扰动 不 灵敏 .无须 系统 在 线 辨识 .物理 实现 简单 等 优点 。 这 种 控制 方法 
通过 控制 量 的 切换 使 系统 状态 沿 着 滑 模 面 滑动 ,使 系统 在 受到 参数 摄 动 和 外 干扰 的 时 候 具 
有 不 变性 , 正 是 这 种 特性 使 得 变 结构 控制 方法 在 机 器 人 控制 中 得 到 广泛 的 应 用 。 

(8) 反 演 控制 设计 方法 : 其 基本 思想 是 将 复杂 的 非 线性 系统 分 解 成 不 超过 系统 阶 数 的 
子 系统 ,然后 为 每 个 子 系统 分 别 设 计 李 雅 普 诺 夫 郧 数 和 中 间 虚 拟 控制 量 ,一直 "后 退 ? 到 整个 
系统 ,直到 完成 整个 控制 律 的 设计 。 利 用 反 演 控制 技术 设计 机 器 人 控制 器 ,可 以 解决 系统 中 
的 非 匹 配 不 确定 性 。 通 过 在 虚拟 控制 中 引入 微分 阻尼 项 ,可 有 效 地 改善 系统 动态 性 能 ; i 
过 在 虚拟 控制 中 引入 模糊 系统 或 神经 网 络 , 可 实现 无 须 建 模 的 自 适应 反 演 控制 ; 通过 在 虚 
拟 控 制 中 引入 切换 函数 ,可 实现 具有 滑 模 控制 特性 的 反 演 控制 。 


1.1.2 不 确定 机 器 人 系统 的 控制 


机 融 人 控制 系统 的 主要 目的 是 通过 给 定 各 关节 的 驱动 力矩 ,使 得 机 器 人 的 位 置 .速度 等 
状态 变量 跟踪 给 定 的 理想 轨迹 。 与 一 般 的 机 械 系 统一 样 , 当 机 器 人 的 结构 及 其 机 械 参 数 确 
定 以 后 ,其 动态 特性 将 由 运动 方程 即 数学 模型 来 描述 。 因 此 ,可 以 应 用 自动 控制 理论 所 提供 
的 设计 方法 ,基于 数学 模型 来 设计 机 器 人 的 控制 器 。 

在 实际 工程 中 要 想得到 精确 的 数学 模型 是 十 分 困难 的 ,因此 在 建立 机 器 人 的 数学 模型 
时 ,需要 做 合理 的 近似 处 理 , 忽 略 一 些 不 确定 性 因素 ,这 些 不 确定 因素 包括 以 下 几 方 面 : 

(1) 参数 不 确定 性 : 例如 ,负载 质量 . 连 杆 质量 .长 度 及 连 杆 质心 等 物理 量 未 知 或 部 分 
已 知 。 

(2) 非 参 数 不 确 定性 : 高 频 未 建 模 动态 ,包括 驱动 器 动力 学 .结构 共振 模式 等 : 低频 未 
建 模 动态 ,包括 动 /静摩擦 力 ,关节 柔性 等 。 

(3) 作业 环境 干扰 .驱动 吉 饱 和 测量 误差 、 舍 人 误差 及 采样 延 时 等 因素 。 

上 述 因 素 的 存在 可 能 会 引起 控制 系统 质 的 变化 ,甚至 成 为 系统 不 稳定 的 原因 。 

应 用 于 不 确定 性 机 器 人 的 先进 控制 策略 可 分 为 三 大 类 , 即 自 适应 控制 . 变 结构 控制 
和 重 棒 控制 。 通 过 与 自 适应 控制 、 变 结构 控制 和 和 鲁 棒 控制 方法 相 结 合 ,PID 控制 ,神经 网 
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络 控制 .模糊 控制 .和 迭代 学 习 控 制 和 反 演 控制 方法 也 可 以 实现 对 不 确定 机 器 人 系统 的 精 
确 控制 。 


1.2 机 器 人 动力 学 模型 及 其 结构 特性 


一 个 典型 的 多 关节 机 器 人 如 图 1-1 所 示 。 
考虑 一 个 nn 关节 机 器 人 .其 动态 性 能 可 由 二 阶 非 线性 微分 方程 描述 .: 
M(q)0q --C(q.d)d +G) HEG) - t, —c (4. D 
其 中 .gE R" HRPA. Maq) ER” HULARA WERE E.C. g) ER" 表示 离心 
HAFRH-GU) ER 为 重力 项 ,F (4 )ER” 表示 摩擦 力矩 ,r ER 为 控制 力矩 ,rsER" 为 
外 加 扰动 。 
机 器 人 系统 的 动力 学 特性 如 下 : 
(D M(q)—2C(G ,6 ) 是 一 个 斜 对 称 矩阵 ,xTCM(qg) 一 2C(q,q ))x 一 0。 
(2) 惯性 矩阵 M(9d ) 是 对 称 正 定 和 矩阵 ,存在 正 数 m, ,m, :满足 如 下 不 等 式 : 
my læ l Sr MG S m lx’ (1.2) 
(3) 存在 一 个 依赖 于 机 械 手 参数 的 参数 向 量 ,使 得 M (D CC q 4 G), F Cq ) 满 足 线 
性 关系 : 
M(q09 -- C(q.qD? p -- G(q) +F) —9 (q.d. p :9)P (1.3) 
Kip. (q.d .9.p) € R'" Jj CLIE T5 2E lt p RA [d URL AB [V «E Je Bd ALT CAS RH 
BTT RUHO CLA PROB e «IP. € R” 是 描述 机 器 人 质量 特性 的 未 知 定常 参数 向 量 ， 
一 个 典型 的 双关 节 刚 性 机 械 手 示意 图 如 图 1-2 所 示 。 本 书 中 的 大 多 数 仿真 实例 都 采用 
该 机 械 手 进行 验证 。 


图 1-1 典型 的 多 关节 机 器 人 图 1-2 典型 的 双关 节 刚 性 机 械 手 示意 图 


1.3 基于 S 函数 的 Simulink 仿真 


S 函数 是 Simulink 的 重要 部 分 , 它 为 Simulink 环境 下 的 仿真 提供 了 强 有 力 的 拓展 能 
力 。S 函数 可 以 用 计算 机 语言 来 描述 动态 系统 。 在 控制 系统 设计 中 ,S 函数 可 以 用 来 描述 
控制 算法 、 自 适应 算法 和 模型 动力 学 方程 。 
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S 函数 中 使 用 文本 方式 输入 公式 和 方程 ,适合 复杂 动态 系统 的 数学 描述 ,并 且 在 仿真 过 
程 中 可 以 对 仿真 参数 进行 更 精确 的 描述 。 在 本 书 的 机 器 人 控制 系统 的 Simulink 仿真 中 , 主 
要 使 用 S 函数 来 实现 控制 律 . 自 适应 律 和 被 控 对 象 的 描述 。 


1.8.1 S 函数 简介 
S 函数 模块 是 整个 Simulink 动态 系统 的 核心 ,也 可 以 说 S 函数 是 Simulink 最 具 魅 力 的 
地 方 。 


S 函数 是 系统 男 数 (Csystem function) 的 简称 ,是 指 采 用 非 图 形 化 的 方式 ( 即 计算 机 语 
言 ,区 别 于 Simulink 的 系统 模块 ) 描 述 的 一 个 功能 块 。 用 户 可 以 采用 MATLAB 代码 、C、 
C++ 等 语言 编写 S 函数 。S 函数 由 一 种 特定 的 语法 构成 ,用 来 描述 并 实现 连续 系统 、 离 散 系 
统 以 及 复合 系统 等 动态 系统 ,S 函数 能 够 接收 来 自 Simulink 求解 器 的 相关 信息 ,并 对 求解 
器 发 出 的 命令 做 出 适当 的 响应 ,这 种 交互 作用 非常 类 似 于 Simulink 系统 模块 与 求解 器 的 交 
互 作用 。 一 个 结构 体系 完整 的 S 函数 包含 了 描述 动态 系统 所 需 的 全 部 能 力 , 所 有 其 他 的 使 
用 情况 都 是 这 个 结构 体系 的 特例 。 


1.3.2 S 函数 使 用 步骤 


一 般 而 言 ,S 函数 的 使 用 步 又 如 下 : 

(1) 创建 S 函数 源 文件 。 创建 S 函数 源 文件 有 多 种 方法 ,Simulink 提供 了 很 多 S 函数 
模板 和 例子 ,用 户 可 以 根据 自己 的 需要 修改 相应 的 模板 或 例子 即 可 。 

(2) 在 动态 系统 的 Simulink 模型 框图 中 添加 S-function 模块 ,并 进行 正确 的 设置 。 

(3) 在 Simulink 模型 框图 中 按照 定义 好 的 功能 连接 输入 输出 端口 。 

为 了 方便 S 函数 的 使 用 和 编写 , Simulink 的 Functions&Tables 模块 库 还 提供 了 
S-function demos 模块 组 ,该 模块 组 为 用 户 提 供 了 编写 S 函数 的 各 种 例子 ,以 及 S 函数 模板 
模块 。 


1.3.3 S 函数 的 基本 功能 及 重要 参数 设 定 


S 函数 的 基本 功能 及 重要 参数 设 定 如 下 : 

(1) S 函数 功能 模块 : 各 种 功能 模块 完成 不 同 的 任务 ,这 些 功能 模块 (函数 ) 称 为 仿真 例 
程 或 回调 函数 (call-back functions) ,包括 初始 化 (initialization) ,导数 (mdlDerivative) .输出 
(mdlOutput) 等 。 

(2) NumContStates 表示 S- 函 数 描述 的 模块 中 连续 状态 的 个 数 。 

(3) NumDiscStates 表示 离散 状态 的 个 数 。 

(4) NumOutputs 和 NumInputs 分 别 表示 模块 输出 和 输入 的 个 数 。 

(5) 直接 馈 通 (dirFeedthrough) 为 输入 信号 是 否 在 输出 端 出 现 的 标识 , 取 值 为 0 或 1。 
例如 , 形 如 y — kou. 的 系统 需要 输入 ( 即 直 接 反馈 ) ,其 中 ,w 是 输入 ,k 是 增益 ,> 是 输出 , 形 
MER y= 二 z+ ,zx 二 wu 的 系统 不 需要 输入 ( 即 不 存在 直接 反馈 ), 其 中 ,x 是 状态 ,w 是 输入 ,y 
为 输出 。 

(6) NumSampleTimes 为 模块 采样 周期 的 个 数 ,S 郴 数 支 持 多 采样 周期 的 系统 。 

除了 sys 外 ,还 应 设置 系统 的 初始 状态 变量 co WHER str 和 采样 周期 变量 to t, 变 
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量 为 双 列 矩 阵 ,其 中 ,每 一 行 对 应 一 个 采样 周期 。 对 连续 系统 和 单个 采样 周期 的 系统 来 说 ， 
该 变量 为 [i ,z:], 为 采样 周期 ,t, — — 1 表示 继承 输入 信号 的 采样 周期 ,1, 为 偏 移 量 ,一 般 
取 为 0。 对 连续 系统 来 说 ,t, 取 为 [一 1,0]。 


1.3.4 S 函数 描述 实例 


在 控制 系统 设计 中 ,S 函数 可 以 用 于 控制 器 . 自 适应 律 和 模型 描述 。 以 模型 JO = 
u 十 di) 的 S 函数 描述 为 例 ,其 中 ,u 为 控制 输入 ,d (2) 为 加 在 控制 输入 端的 扰动 ,模型 输出 
为 9 和 6 , 即 转动 角度 和 转动 角速度 ,J 为 转动 惯量 。 该 模型 可 描述 如 下 : 


Ži =z; 
5 1 
Wein --dGQ)) 


Ru. —0.:,—Ó. 
针对 该 模型 的 S 函数 描述 介绍 如 下 : 
(1) 模型 初始 化 Initialization 函数 。 


采用 S 函数 描述 动力 学 方程 ,可 选取 1 输入 2 输出 系统 ,如 果 角 度 0 和 角速度 6 的 初始 
值 取 零 , 则 模型 初始 化 参数 写 为 [0,0] ,模型 初始 化 S | C S UT 


function [sys, x0, str,ts] = mdlInitializeSizes 
sizes = simsizes; 
sizes. NumContStates 
sizes.NumDiscStates 
sizes.NumOutputs 
sizes.NumInputs 
sizes.DirFeedthrough 


e O He No N 
up ABD Wa a NS NX 


sizes.NumSampleTimes 


sys = simsizes(sizes); 


x0 = [0,0]; 
str = []; 
ts - [00]; 


(2) 微分 方程 描述 的 mdlDerivative 函数 。 

mdlDerivative 函数 可 用 于 描述 微分 方程 并 实现 数值 求解 。 在 控制 系统 中 ,可 采用 该 函 
数 来 描述 被 控 对 象 和 自 适应 律 等 ,并 通过 Simulink 环境 下 选择 数值 分 析 方 法 (如 ODE 77 
法 ) 实 现 模型 的 数值 求解 。 取 J —2.d G) — sint , 则 采用 S 函数 可 实现 该 模型 角度 9 和 角 速 


度 6 的 求解 ,描述 如 下 : 


function sys = ndlDerivatives(t,x,u) 
J72; 

dt = sin(t); 

ut-u(1); 

sys(1) = x(2); 

sys(2) = 1/J * (ut * dt); 
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(3) 用 于 输出 的 mdlOutput 函数 。 

S PR Zi WJ mdlOutput 函数 通常 用 于 描述 控制 器 或 模型 的 输出 。 采 用 SS 函数 的 
mdlOutput 模块 来 描述 模型 角度 0 和 角速度 6 的 输出 : 

function sys = mdlOutputs(t, x, u) 

sys(1) = x(1); 

sys(2) » x(2); 


PoE 机 械 手 PD 控制 


CHAPTER 2 


2.1 机 械 手 独立 PD 控制 


2.1.1 控制 律 的 设计 
当 忽 略 重力 和 外 加 干扰 时 ,采用 独立 的 PD 控制 ,能 满足 机 械 手 定点 控制 的 要 求 …… 。 
设 n 关节 机 械 手 方程 为 
D(q0q --C(q.q)0q =t (2.1) 
其 中 ,D(q) 为 n Xn MEERE. C. qg) H no n 阶 离心 和 哥 氏 力 项 。 
独立 的 PD 控制 律 为 
r —K,é--K,e (2. 2) 
取 跟 踊 误 差 为 e 二 qs 一 gq ,采用 定点 控制 时 ,gq, 为 常 值 , 则 q ,二 49 二 0。 
此 时 ,机械手 方程 为 
D (q)(4,—4) -C(q.d)(d,—d) -K4é +K,e=0 
亦 即 
D (q)é -- C(q .4)é -- K,e — — K,é (2.3) 
取 Lyapunov (E H EWR) AO 
l sa TEE 
Vet D(q)é 4- 2* Ke 
由 D) X K, 的 正定 性 知 ,V 是 全 局 正定 的 , 则 
V=é Dé-- L6 Dé Hé" K,e 


利用 DD 一 2C In got sir é "Dé =2ė "Cé , 则 
V—é'Dé--é'Cé -é'" K,e—é" (Dé--Cé -K,e) ——é"K,. é«0 
2.1.2 收敛 性 分 析 
由 于 立 是 半 负 定 的 , 且 天 ,为 正定 , 当 V=0 时 ,有 6 三 0, 从 而 二 90。 代入 方程 (2. 3), 


有 开 ,e 王 0, 再 由 开 , 的 可 道 性 知 e 二 0。 由 LaSalle EFE ^ Al. Cee) — (0,00 J& 2€ T8 BLBR-F- 
Ja) Br dE E E II SE: lg x. BA EXE SIR TR EF Cao qo) HL 8 tokt sg >q 0770. 
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2.1.3 仿真 实例 


针对 被 控 对 象 式 (2. D , 选 二 关节 机 械 手 系统 (不 考虑 重力 ERATA 
型 为 
D(q0q--C(q.q)q =T 
其 中 ， 
ue + 2p4cosq. Loud 
D(q)— 
ba pscosg; P» 
— p4Qssinq; — ps(Cq, 十 gy)sing， 
co -| q biqi 42 " 
P3q181nq» 0 
取 p-[2.90 0.76 0.87 3.04 0.87] ,q,—[0.0 0.0]',q,—[0.0 0.0]'. 


n 100 0 
位 置 指令 为 gu(0) 一 [L1.0 L0] AERE BERE C2 DOCK, = | MEX 


jn i | ,仿真 结果 如 图 2-1 和 图 2-2 所 示 。 
0 100 


position tracking of link 1 


time(s) 
1 p 
N 
x 
SS 0.8 
Qi 
o 
p 0.6 
ET 
8 04 
z 
£ 0.2 
& 0 
0 1 2 3 4 5 6 7 8 9 10 
time(s) 


图 2-1 3X 71 E BEI D EU IS RAR 


仿真 中 , 当 改 变 参数 K, Ka 时 ,只 要 满足 K> 0K, > 0, A AEIR EC BE E 89 05 E 
完全 不 受 外 力 , 没 有 任何 干扰 的 机 械 手 系统 是 不 存在 的 ,独立 的 PD 控制 只 能 作为 基础 来 考 
虑 分 析 , 但 对 它 的 分 析 有 重要 意义 。 


第 2 章 机 械 手 PD 控制 P 9 


toll 


tol2 


time(s) 


图 2-2 独立 PD 控制 的 控制 输入 仿真 结果 


仿真 程序 如 下 : 
(1) Simulink 主 程 序 : chap2_1sim. mdl。 


itii | i 


Clock To Workspace 


Position2 


(2) 控制 器 子 程序 : chap2 lctrl. m, 
function [sys,xO,str,ts] = spacemodel(t,x,u, flag) 


switch flag, 
case 0, 
[ sys, x0, str, ts] = ndlInitializeSizes; 
case 3, 
sys = ndlOutputs(t,x,u); 
case (2,4,9) 
sys-[]; 
otherwise 
error(['Unhandled flag = ',num2str(flag)]); 
end 
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Mtn 


function [sys, x0, str,ts] = mdlInitializeSizes 
Sizes = simsizes; 

sizes.NumOutputs = 
sizes.NumInputs 
sizes.DirFeedthrough - 


1 
HON 


外 


sizes.NumSampleTimes 


sys = simsizes(sizes); 
x0 = [] 

str = []; 

ts = [00]; 


function sys = mdlOutputs(t, x, u) 
R1-u(1);dr1-20; 
R2-u(2);dr2-70; 


x(1) = u(3); 
x(2) = u(4); 
x(3) = u(5); 
x(4) = u(6); 
el-R1-x(1); 
e2- R2- x(3); 
= [e1;e2]; 


del = dri - x(2); 
de2 = dr2- x(4); 
de = [de1;de2]; 


Kp - [30 0;0 30]; 
Kd = [30 0;0 30]; 


tol = Kp* e+ Kd * de; 


sys(1) = tol(1); 
sys(2) = to1(2); 


(3) 被 控 对 象 子 程序 ; chap2. Iplant. m, 


% S- function for continuous state equation 
function [sys,xO,str,ts] = s function(t, x,u, f lag) 


switch flag, 
% Initialization 
case 0, 
[sys, x0, str, ts] = mdlInitializeSizes; 
case 1, 
sys = mndlDerivatives(t,x,u); 
% Outputs 
case 3, 
sys = mdlOutputs( t, x, u); 
% Unhandled flags 
case (2, 4, 9} 
sys — []; 
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* Unexpected flags 
otherwise 
error(['Unhandled flag = ',num2str(flag)]); 
end 


* mdlInitializeSizes 
function [sys, x0, str,ts] = mdlInitializeSizes 
global pg 

sizes = simsizes; 
sizes.NumContStates 


Li 
Oct nx o 心 


sizes.NumDiscStates 
sizes.NumOutputs 
sizes.NumInputs 


sizes.DirFeedthrough 


sizes. NumSampleTimes 
Sys = simsizes(sizes); 


x0-[000 0]; 
strz[]; 
ts-[1; 


p= [2.9 0.76 0.87 3.04 0.87]; 
g79.8; 

function sys = mdlDerivatives(t, x,u) 
global p g 


DO = [p(1) * p(2) * 2 * p(3) * cos(x(3)) p(2) + p(3) * cos(x(3)); 
p(2) * p(3) * cos(x(3)) p(2)]; 

CO-[-p(3)*x(4)*sin(x(3)) -p(3) * (x(2) * x(4)) * sin(x(3)); 
p(3)*x(2) *sin(x(3)) 0]; 

tol-u(1:2); 

dq= [x(2);x(4) ]; 


S= inv(D0) * (tol- CO * dq); 


sys(1) = x(2); 
sys(2) = S(1); 
sys(3) = x(4); 
sys(4) = S(2); 
function sys = mdlOutputs(t, x,u) 
sys(1) = x(1); 
sys(2) = x(2); 
sys(3) = x(3); 
sys(4) = x(4); 


(4) 绘图 子 程序 : chap2 lplot. m, 


close all; 


figure(1); 

subplot(211); 

plot(t,x1( 3,1), "£^, £;x1(5,2); 'b'y; 
xlabel('time(s)');ylabel('position tracking of link 1'); 
subplot(212); 

plot(t,x2(:,1),'r',t,1»2(:,2), 'b'); 
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xlabel('time(s)');ylabel('position tracking of link 2'); 


figure(2); 

subplot(211); 
plot(t,tol(:,1), 'r'); 
xlabel('time(s)');ylabel('toll'); 
subplot(212); 
plok(t,tol(:,2),' xt); 
xlabel('time(s)');ylabel('tol2'); 


2.2 基于 重力 补偿 的 机 械 手 PD 控制 


2.2.1 控制 律 的 设计 


当 考 虑 重力 时 ,采用 基于 重力 补偿 的 PD 控制 ,能 满足 机 械 手 定点 控制 的 要 求 号 。 
Wn 关节 机 械 手 方程 为 
D(q)0q - C(q.q)q + G(q) —c (2. 4) 
其 中 ,D(q) 为 n Xn 阶 正定 惯性 矩阵 ,C(qg ,9 ) 为 n Xn 阶 离 心 和 哥 氏 力 项 ,G(d ) 为 重力 矩 向 量 。 
基于 重力 补偿 的 PD 控制 律 为 


r —K,é + K,e --G() (2.5) 


其 中 ,G(d ) 为 对 重力 矩 的 估计 值 。 
取 跟 踪 误 差 为 e 二 qs 一 g ,采用 定点 控制 时 ,q。 为 常 值 , 则 4 ,二 9g, 三 0。 
此 时 ,机 械 手动 力学 方程 为 
D(q)(q,—q)-C(q.d)(q,—q) HK +K,e 6G )—G(q)-0 


2.2.2 控制 律 分 析 


控制 律 式 (2.5) 的 实现 关键 在 于 对 重力 矩 G(q) 的 估计 ,针对 重力 矩 的 估计 方法 有 以 下 
两 大 类 。 
(1) 当 对 重力 矩 的 估计 值 准 确 时 ,G(q)=G(q), 有 
D(q)0e + (C(q.q) - K))é 3- K,e —0 (2. 6) 
此 时 ,控制 的 稳定 性 和 收敛 性 分 析 过 程 同 2. 1 节 的 “机 械 手 独立 PD 控制 ”。 
(2) 当 对 重力 矩 的 估计 值 不 准确 时 ,需要 设计 重力 补偿 算法 。 目 前 ,有 代表 性 的 重力 补 
偿 PD 控制 方法 有 以 下 两 种 : 
。 在 线 估 计 重 力 补偿 的 PD 控制 : 文献 [3] 针 对 双 和 柔性 关节 机 械 臂 ,设计 了 在 线 估计 重 
力 的 自 适应 算法 ,实现 了 基于 在 线 重力 补偿 的 PD 控制 。 
。 具 有 固定 重力 补偿 的 PD 控制 : 由 于 在 线 估计 重力 补偿 项 G(q) 会 加 重 计算 机 实时 
计算 的 负担 ,为 此 ,Takegaki 等 趾 采 用 事先 计算 出 的 固定 重力 项 作为 补偿 ,采用 增 
加 反馈 增益 来 减 小 稳 态 误差 ,并 采用 系统 的 Hamilton 函数 作为 其 李 雅 普 诺 夫 函数 ， 
证 明了 该 方法 的 稳定 性 和 收敛 性 。 
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2.3 基于 模型 补偿 的 机 械 手 PD 控制 


2.3.1 系统 描述 


双关 节 机 械 臂 动力 学 方程 可 写 为 
H(q0q — C(q.q)0qd + G(q) =r (2. 7) 
其 中 ,gq 二 [gi q:] rz 三 [ri til. 


2.3.2 控制 器 的 设计 


定义 d.—4d.—A4.4,—4.—A q GTETEES EROS 
s—q--Aq (2,87 
其 中 ,A 是 一 个 常数 阵 ,A 0. 
设计 基于 模型 补偿 的 PD 型 控制 律 为 
t — Hd, -- Cá, --G — Ks (2.9) 
其 中 ,Kb 为 正定 矩阵 。 
构造 李 雅 普 诺 夫 函 数 


I 
VG)—s'Hs 


Va) —5' Hi +s" Hs 


=s'(Hg HI)+5s Hs 


=s'(r—Cį—G Hj) t s Hs 

—s (Hd, - Cd, +G — Kos — C(s t4.) —G — Hd) t 5! Hs 
T l T 了 

=§ (Kos — Cs) s Hs 


=—Kps's ts b 20s 
— —K,s's «o0 
由 于 V 是 半 负 定 的 , 且 Kb 为 正定 的 , 当 VV 三 0 BL s=0, Am q—0.4—0, H 


LaSalle $E F^ Al, (d ,4) 二 (0,0) 是 受 控 机 械 手 全 局 渐进 稳定 的 平衡 点 , 即 从 任意 初始 条 件 
Codo) HE% cmo] A qoid du. 


2.3.3 仿真 实例 


二 关节 平面 机 械 臂 具有 4 个 物理 参数 ” ,分 别 为 质量 m, ,转动 惯量 [. ,质量 中 心 距 第 二 
关节 处 的 距离 /.. ,质量 中 心 与 第 二 机 械 辟 的 夹 角 8.。 机 械 臂 的 实际 物理 参数 值 见 表 2-1。 
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表 2-1 机 械 臂 的 实际 物理 参数 值 


mi/kg l,/m la/m Il,/kg | m,/kg lx/m I./kg à, ei 


ez 


] 1 1/2 1/12 3 l 2/5 0 —7/12 9. 81 


被 控 对 象 取 式 (2.7) ,两 力 辟 机 械 手 的 位 置 指令 分 别 为 qa = sinnt) squ = sin rt). 


5 
RRM D 一 | nd a-i ;| ,第 一 关节 和 第 二 关节 的 位 置 及 速度 中 中 


及 控制 输入 仿真 结果 如 图 2-3 一 图 2-5 所 示 。 


E 
S ideal signal 
S tracking signal 
ap 
[-] 
E 
E 
P 
2 
: 3 
time(s) 
x 
E ideal signal 
* tracking signal 
E 
z 
E 
oo 
E 
2 
E 
E 
0 0.5 | 1.5 2 2.5 3 
time(s) 
图 2-3 第 一 关节 的 位 置 及 速度 跟踪 仿真 结果 
N 
E ideal signal 
5 tracking signal 
oD 
x 
E 
z 
E 
E 
& 
0 0.5 1 1.5 2 2.5 3 
time(s) 


ideal signal 
tracking signal 


velocity tracking of link 2 


0 0.5 1 L5 2 2.5 3 
time(s) 


A 2-4 第 二 关节 的 位 置 及 速度 跟踪 仿真 结果 
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control input of link 1 
control input of link 2 


control input of link 1 and link 2 


0 0.5 1 1.5 
time(s) 


图 2-5 第 一 和 第 二 关节 的 控制 输入 仿真 结果 


N 


2.5 3 


仿真 程序 如 下 : 
(D Simulink 主 程序 : chap2 2sim. mdl, 


chap2 2input 


SFundion1 


chap2 2d" chap2 2plant 


S-Function4 


Go To Workspace 


Clock 


(2) 输入 指令 程序 ; chap2 2input. m, 


function [sys,xO,str,ts] = input(t,x,u,flag) 


switch flag, 
case 0, 
[sys, x0, str, ts] = ndlInitializeSizes; 
case 3, 
sys = ndlOutputs(t,x,u); 
case {2,4,9} 
sys- []; 
otherwise 
error([ 'Unhandled flag = ',num2str(flag)]); 
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end 


function [sys, x0, str, ts] = mdlInitializeSizes 
Sizes - simsizes; 

sizes.NumOutputs - 6; 
sizes.NumInputs - 0; 
sizes.DirFeedthrough 
sizes.NumSampleTimes 


0; 
0; 


SyS 7 simsizes(sizes); 


x0 - [] 
str = []; 
ts eL 


function sys = mdlOutputs(t, x, u) 

ql d= sin(2* pi * t); 

q2 d= sin(2* pi * t); 

dql_d= 2 * pi * cos(2 * pi* t); 
dq2_d= 2 * pi * cos(2 * pi * t); 

ddql d= - (2 * pi)^2 * sin(2*pi*t); 
ddq2 d= - (2*pi)^2*sin(2*pi*t); 


sys(1)= ql_d; 
sys(2) = dal d; 
sys(3) = ddql d; 
sys(4) = q2 d; 
sys(5) = dq2 d; 
sys(6) = ddq2 d; 


(3) 控制 器 程序 : chap2 2ctrl. m. 


function [sys,x0,str,ts] = control strategy(t,x,u, flag) 
switch flag, 
case 0, 
[sys, x0, str, ts] = mdlInitializeSizes; 
case 3, 
sys = mdlOutputs(t,x,u); 
case (2,4,9) 
sys- []; 
otherwise 
error(['Unhandled flag = ',num2str(flag)]); 
end 


function [sys, x0, str, ts] = mdlInitializeSizes 


sizes = simsizes; 


LU 
N 
EN 


sizes.NumOutputs 
sizes.NumInputs = 10; 
sizes.DirFeedthrough = 1; 
sizes.NumSampleTimes - 0; 
SyS 7 simsizes(sizes); 

x0 El; 

str = [ly 
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ts = []; 
function sys = mdlOutputs(t, x, u) 
alfa = 6. 7;beta = 3. 4;epc = 3.0;eta = 0; 


ql_d=u(1);dql_d= u(2);ddql d= u(3); 
q2_d = u(4);dq2_d = u(5);ddq2 d= u(6); 
ql =u(7);dqi = u(8); 

q2 = u(9);dq2 = u(10); 


ml =1;11=1; 

lc1 = 1/2; I1 = 1/12; 

g=9.8; 

el-ml*]1li1*]lci- I1 =mi * ]11^2; 
e2 = g/11; 


dq d= [dq1_d, dq2_d]'; 
ddq d= [ddq1 d,ddq2 d]'; 


q error-[q1-q1 d,q2-q2 d]'; 
dq error = [dql - dgl d,dq2 - dq2 d]'; 


H= [alfa + 2 * epc * cos(q2) * 2 * eta * sin(q2), beta + epc * cos(q2) + eta * sin(q2); 
beta + epc * cos(q2) + eta * sin(q2), beta]; 

C-[(-2*epc* sin(q2) * 2* eta * cos(q2)) * dq2, ( - epc * sin(q2) + eta * cos(q2)) * dq2; 
(epc * sin(q2) - eta * cos(q2)) * dq1,0]; 

G= [epc * e2 * cos(q1 + q2) + eta * e2 * sin(ql * q2) + (alfa- beta + el) * e2 * cos(q1); 
epc * e2 * cos(q1 + q2) + eta * e2 * sin(q1 + q2)]; 


Fai-5* eye(2); 

dqr = dq d- Fai*q error; 

ddqr = ddq d- Fai* dq error; 

s = Fai * q error dq error; 

Kd = 100 * eye(2); 

tol =Hx ddqr +C *# dqr + G- Kd * s; 
sys(1) = tol(1); 
sys(2) = tol(2); 


(4) 被 控 对 象 程序 : chap2 2plant. m, 


function [sys, x0, str,ts] = s function(t, x,u, flag) 
switch flag, 
case 0, 
[ sys, x0, str, ts] = ndlInitializeSizes; 
case 1, 
sys = ndlDerivatives(t, x,u); 
case 3, 
sys = ndlOutputs(t, x, u); 
case {2, 4, 9 } 
sys = []; 
otherwise 
error(['Unhandled flag = ',num2str(flag)]); 
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end 


function [sys, x0, str,ts] = ndlInitializeSizes 


sizes = simsizes; 


I 


sizes.NumContStates 
sizes.NumDiscStates  - 
sizes. NumOutputs - 
sizes. NumInputs = 


sizes.DirFeedthrough = 


Oo O N Ò © » 
~ 


sizes. NumSampleTimes = 
sys = simsizes( sizes); 
x0= [1.0,0,1.0,0]; 
str-[]; 

tss LT; 

function sys = mdlDerivatives(t, x,u) 
tol - [u(1);u(2)]; 

ql= x(1); 

dql = x(2); 

q2 = x(3); 

dq2 = x(4); 


alfa= 6.7; 
beta = 3.4; 
epc = 3. 0; 
eta= 0; 


ml = 1;11=1; 

lcl = 1/2; 11 = 1/12; 

g=9.8; 
el-ml*11*1lcl-I1-ml1*]11^2; 
e2 = g/11; 


H= [alfa + 2 * epc * cos(q2) * 2 * eta * sin(q2), beta + epc * cos(q2) + eta * sin(q2); 
beta * epc * cos(q2) * eta * sin(q2), beta]; 

C-[(-2* epc * sin(q2) * 2 * eta * cos(q2)) * dq2, ( - epc * sin(q2) + eta * cos(q2)) * dq2; 
(epc * sin(q2) - eta * cos(q2)) * dq1,0]; 

G= [epc * e2 * cos(q1 + q2) + eta * e2 * sin(ql * q2) + (alfa- beta + e1) * e2 * cos(q1); 
epc * e2 * cos(q1 + q2) + eta * e2 * sin(q1 * q2)]; 

S= inv(H) * (tol ~ C* [dq1;dq2] - G); 


sys(1) - x(2); 
sys(2) = S(1); 
sys(3) = x(4); 
sys(4) = S(2); 
function sys = mdlOutputs(t,x,u) 
sys(1) = x(1); 
sys(2) = x(2); 
sys(3) = x(3); 
sys(4) 7» x(4); 
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(5) 作 图 程序 : chap2_2plot. m, 


close all; 


figure(1); 

subplot(211); 

plot(t,qd(:,1), 'r', t, q(:, 1), 'b', 'linewidth',2); 
xlabel('time(s)');ylabel('position tracking of link 1"); 
legend('ideal signal', 'tracking signal'); 

subplot(212); 

plot(t,qd(:,2), 'r', t, q(:,2), 'b', 'linewidth',2); 
xlabel('time(s)');ylabel('velocity tracking of link 1'); 
legend('ideal signal', tracking signal'); 


figure(2); 

subplot(211); 

plot(t,qd(:,4), 'r',t,q(:,3), 'b', 'linewidth',2); 
xlabel('time(s)');ylabel('position tracking of link 2'); 
legend('ideal signal', 'tracking signal'); 

subplot(212); 

plot(t,qd(:,5), 'r',t,q(:,4), 'b', 'linewidth',2); 
xlabel('time(s)');ylabel('velocity tracking of link 2'); 
legend('ideal signal', tracking signal'); 


figure(3); 

plot(t,tol(:,1), r',t,tol(:,2), 'b', 'linewidth',2); 
xlabel('time(s)');ylabel('control input of link 1 and link 2'); 
legend('control input of link 1','control input of link 2'); 
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机 械 手 神经 网 络 目 适应 控制 


CHAPTER 3 


如 果 被 控 对 象 的 数学 模型 已 知 , 滑 模 控 制 器 可 以 使 系统 输出 直接 跟踪 期 望 指令 ,但 较 大 
的 建 模 不 确定 性 需要 较 大 的 切换 增益 ,这 就 造成 拌 振 , 抖 振 是 滑 模 控制 中 难以 避免 的 问题 。 

将 滑 模 控制 结合 神经 网 络 双 近 用 于 非 线 性 系统 的 控制 中 ,采用 神经 网 络 实现 模型 未 知 
部 分 的 自 适应 逼近 ,可 有 效 地 降低 切换 增益 。 神 经 网 络 自 适 应 律 通过 Lyapunov 方法 导出 ， 
通过 自 适 应 权重 的 调节 保证 整个 闭环 系统 的 稳定 性 和 收敛 性 。 

RBF( Radial Basis Function. 1% [1] JE PR ŽO di £5 Ie] 4 F 1988 年 提出 。 相 比 多 层 前 馈 BP 
(Back Propagation , 反 向 传播 ) 网 络 ,RBF 网 络 由 于 具有 和 良好 的 泛 化 能 力 , 网 络 结构 简单 , 避 
免 不 必要 和 元 长 的 计算 而 备 受 关注 。 关 于 RBF 网 络 的 研究 表明 RBF 神经 网 络 能 在 一 个 紧 
凑 集 和 任意 精度 下 , 允 近 任何 非 线 性 函数 ""。 目 前 ,已 经 有 许多 针对 非 线 性 系统 的 RBF 神 
经 网 络 控 制 研 究 成 果 发 表 。 


3.1 一 种 简单 的 RBF 网 络 自 适应 滑 模 控制 


3.1.1 问题 描述 
考虑 一 种 简单 的 动力 学 系统 : 
0=f(0,0) +u (3. 1) 
其 中 ,0 为 转动 角度 ,zx 为 控制 输入 。 
写成 状态 方程 形式 为 
yi (3.2) 
=f (+ü 


Kp —0.2,—0. f(r) ARM. 
角度 指令 为 x,, 则 误差 及 其 导数 为 
xr E St i 
定义 滑 模 函数 
$-—ce-Fé. c>o (3. 3) 
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s —cé 十 e 一 ce zs —rs=ce Hfr) tuia 
由 式 (3.3) 可 见 , 如 果 s 一 0, 则 e 一 0 H. e 一 0 


3.1.2 RBF 网 络 原理 


由 于 RBF 网 络 具 有 万 能 通 近 特性 品 ,采用 RBF 神经 网 络 逼 近 f OO ,网 络 算法 为 
lx —e, ||? 
h, =exp( 


f=W’ h(x)+e 
其 中 ,x 为 网 络 的 输入 ,j 为 网 络 隐 含 层 第 j 个 节点 ,一 [LA 
£ 


W' 为 网 络 的 理想 权 值 , e 为 网 络 的 通 近 误差 ,je| 私 ex 


(3.5) 
网 络 输入 取 x=[r xz 


网 络 的 高 斯 基 函 数 输出 ， 
; 则 网 络 输出 为 


f(x) =Wih(x) 


3.1.3 控制 算法 设计 与 分 析 


(3.6) 
由 于 f(x) 一 f(x)= 


"h(x)--e —W'h(x)— —W'h(x)-e 
定义 Lyapunov 函数 为 


下 
-wÑ 
2y 
,W—W-W', 


(3. 7) 


—scé 二 f(x) --u ta) mW. 
设计 控制 律 为 


u 一 一 c —f (x) -E,— qsgn(s) 
则 


(3. 8) 
lc 
V =s f) —f G0 — gsgnG)) + 7 W 


=s(—WŴ"h(x) +e — psen) +W 


—&Es —13]: |n (ZW — Go) 
取 w>ew, 自 适应 律 为 


W — ysh (x) 


(3.9) 
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Wl V—es—75|s|«0. 

可 见 , 控制 律 中 的 鲁 棱 项 msn GO KY ME JH Je SE TR P Z S] 4 B 3 E 1 28 eA DIE E 
稳定 。 | 

由 于 当 且 仅 当 :==0 时 ,V=0。 即 当 V 夺 0 时 ,s 夺 0。 根据 LaSalle 不 变性 原理 ,闭环 系 
统 为 渐进 稳定 , 即 当 :ce 时 ,*->0。 系 统 的 收敛 速度 取决 于 7。 


BP VO. V0. UM zT LV. 有 界 , 因 此 ,可 以 证 明 屿 有 界 , 但 无 法 保证 网 收敛 


TW. 
3.1.4 仿真 实例 
考虑 如 下 被 控 对 象 
T =f(x)+u 


其 中 ,f(x)==10zx1xs。 

被 控 对 象 的 初始 状态 取 [0.15 0], 位 置 指令 为 xz, 二 sint ,控制 律 采用 式 (3. 8), 自 适应 
律 采用 式 (3.9), 取 y==1500,w 二 1.5。 根 据 网 络 输入 r 和 ns 的 实际 范围 设计 高 斯 基 函 数 
的 参数 中 ,参数 c Mb 取 值 分 别 为 0.5X[ 一 2 一 1 0 1 2] 和 3.0。 仿 真 程序 中 为 了 避 
免 泥 淆 ,将 ;==ce 十 e 中 的 c 写 为 *, 取 A 二 10。 网 络 权 值 中 各 个 元 素 的 初始 值 取 0. 10。 仿 真 
结果 如 图 3-1 和 图 3-2 所 示 。 


1 


SD 
* 0 
g 
加 
© 
B =l 
N 
& 

一 2 l 1 dl —— El "- 

2 4 6 8 10 12 14 16 18 20 
time(s) 

2 
B 
ET 
o 
BO 
"9 
Q 
& 

-2 

0 2 4 6 8 10 12 14 16 18 20 

time(s) 


3-1 角度 和 角速度 跟踪 仿真 结果 
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40 


30 


fapproximation 
© 


-20 
-30 
B 2 4 6 8 10 12 14 16 18 20 
time(s) 
图 3-2 fo RiBurBfó E sm 
仿真 程序 如 下 : 


(1) Simulink 主 程序 : chap3. 1sim. mdl。 


(2) 控制 律 及 自 适应 律 S 函数 : chap3_1lctrl. m. 


function [sys, x0, str,ts] = spacemodel(t, x,u, f lag) 
switch flag, 
case 0, 
[sys, x0, str, ts] = mdlInitializeSizes; 
case 1, 
sys = ndlDerivatives(t,x,u); 
case 3, 
sys = mndlOutputs(t,x,u); 
case (2,4,9) 
sys-[]; 
otherwise 
error(['Unhandled flag = ',nun2str(flag)]); 


end 
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function [sys, x0, str, ts] = mdlInitializeSizes 
global b c lama 
sizes = simsizes; 


sizes. NumContStates 


sizes. NumDiscStates 
sizes. NumOutputs 


sizes.NumInputs 


li 
He HANO 
EN 


sizes.DirFeedthrough 


sizes.NumSampleTimes 


sys = simsizes( sizes); 


x0 = 0.1*ones(1,5); 

str = []; 

ts = [00]; 

c=0.5x[-2 -1012; 
-2 -1 012]; 

b=3.0; 

lama = 10; 


function sys = mdlDerivatives(t, x,u) 
global b c lama 


xd - sin(t); 
dxd = cos(t); 
xl =u(2); 
x2 = u(3); 
e= xl- xd; 
de = x2 - dxd; 


s= lama * e * de; 


W= [x(1) x(2) x(3) x(4) x(5)]'; 


xi-[xl;x2]; 


h= zeros(5,1); 
for j= 1:1:5 
h(j) = exp( - norn(xi- c(:,3))^2/(2* b^2)); 


end 
gama = 1500; 
for i= 1:1;5 


sys( i) = gama * s * h(i); 
end 


function sys = mdlOutputs(t, x,u) 
global b c lama 

xd= sin(t); 

dxd = cos(t); 

ddxd = - sin(t); 


xl = u(2); 
x2 = u(3); 
e-xl- xd; 


de = x2 - dxd; 
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s= lama * e + de; 


W= [x(1) x(2) x(3) x(4) x(5)]; 
xi-[xl;x2]; 


h= zeros(5,1); 
for j721:1:5 
h(j) = exp( - norn(xi- c(:,3))^2/(2* b^2)); 
end 
fn=W* h; 
xite = 1.50; 


% fn=10*xl+x2; % Precise f 
ut = - lama * de + ddxd - fn- xite * sign(s); 


sys(1) = ut; 
sys(2) = fn; 


(3) 被 控 对 象 S 函数 : chap3 lplant. m, 


function [sys, x0, str, ts] = s function(t, x,u, f lag) 
switch flag, 
case 0, 
[ sys, x0, str, ts] = mdlInitializeSizes; 
case 1, 
sys = ndlDerivatives(t,x,u); 
case 3, 
sys = ndlOutputs(t,x,u); 
case (2, 4, 9 } 


sys = []; 
otherwise 

error(['Unhandled flag = ',num2str(flag)]); 
end 


function [sys, x0,str,ts] = mdlInitializeSizes 
sizes - simsizes; 


sizes.NumContStates 


sizes.NumDiscStates 
sizes.NumOutputs 


sizes.NumInputs 


u 


sizes.DirFeedthrough 


u 
OOo t u€ ot 
~ 


sizes. NumSampleTimes 
Sys = simsizes(sizes); 
x0= [0.15;0]; 
str-[]; 

ts-[]; 

function sys = mdlDerivatives(t, x,u) 
ut-u(1); 


f-10*x(1)*x(2); ， 
sys(1) - x(2); 

sys(2) f * ut; 

function sys = mdlOutputs(t, x, u) 
f-10*x(1)*x(2); 
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sys(1) = x(1); 
sys(2) = x(2); 
sys(3) =f; 


(4) 作 图 程序 : chap3. 1 plot. m, 


close all; 


figure(1); 

subplot(211); 

plot(t,x(:,1), 'r',t,x(:,2), 'b', 'linewidth',2); 
xlabel('time(s)');ylabel('position tracking'); 
subplot(212); 

plot(t,cos(t), 'r',t,x(:,3), 'b', 'linewidth',2); 
xlabel('time(s)'); ylabel( speed tracking'); 


figure(2); 


plot(t,f(:,1), 'r', t, £(:,3), 'b', 'linewidth',2); 
xlabel('time(s)');ylabel('f approximation'); 


3.2 基于 RBF 网 络 逼近 的 机 械 手 自 适应 控制 


通过 对 文献 L3] 的 控制 方法 进行 详细 推导 及 仿真 分 析 , 研 究 一 类 机 械 臂 神经 网 络 自 适 应 


控制 的 设计 方法 。 
3.2.1 问题 的 提出 


in 关节 机 械 手 方程 为 
M(q)d --C(q.d)d - G(qD HFG) - vc, —c 


(3. 10) 


其 中 ,M(gq) 为 n Xn BriEZETUPEXBR E «C Cg «q ) 为 n Xn MEERE, G) Jg nox Y 阶 惯性 向 


量 ,F (gq ) 为 摩擦 力 ,t, 为 未 知 外 加 干扰 ,r 为 控制 输入 。 
跟踪 误差 为 
e(1)=qgu(t)—gqg(1) 
r—é--Ae 
其 中 ,A —A'-—0. ll 
à :——r-4j--Ae 
Mr —M(q,—4 —-Aé6) —M(qQ,4-A 6) — Mg 


—M(q,4d-A6e)--Cq --G-FF-Fr,—c 
—M(q,-Ae) —Cr--C(qu--Ae) --G-EF-Fr,—c7 
——Cr-—r--f(ix»-Tr, 

其 中 ,f (x) 二 M(gs 二 A e) 二 C(qi 十 Ae) 十 GF。 


(3, 11) 


(3. 12) 


在 实际 工程 中 ,模型 不 确定 项 f Ox ) 为 未 知 ,为 此 ,需要 对 不 确定 项 f Ox ETT ORLIT 
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采用 RBF 网 络 逼 近 f Oxo ,根据 f(x) 的 表达 式 , 网 络 输入 取 
rele" aj di dil 
设计 控制 律 为 
t= 了 (x) Kr (3. 13) 
Hp, f aD H RBF 网 络 的 估计 值 。 
将 控制 律 式 (3.13) 代 入 式 (3. 12) ,得 


Mr ——Cr =f (x) — K,r - f (x2 o t, 


— — (K, 3- Oor +f (x) +r; — — (OK, +C0)r +% (3. 14) 
Kop.fGo—fGOo—f GD.£,—f t. 
WREX Lyapunov FR 
1 
L=3r J 
则 
L —r'Mr + rM ——r'K.r- Fr" —2C)r+r't, 
= —r'K,r 
这 说 明 在 天 , 固定 条 件 下 ,控制 系统 的 稳定 依赖 于 名 BD FOX J£ IBI IE CT ee, 的 
XN. 
采用 RBF 网 络 对 不 确定 项 三 进 行 通 近 。 理 想 的 RBF 网 络 算法 为 
gy =g 0 | x —e, I 27539 
y =Wọ(x) 
其 中 ,x 为 网 络 的 输入 信号 ,Pp (x) 二 L$， p c $8,j /为 隐 层 节点 的 个 数 ,i 为 网 络 输入 
的 个 数 。 


3.2.2 基于 RBF 网 络 逼 近 的 控制 器 
1. 控制 器 的 设计 
采用 RBF RJ Ur f(x), 则 RBF 神经 网 络 的 输出 为 
fa) =W'p(x) (3. 15) 


W—-W-W, |W] EKW m 
设计 控制 律 为 
dell pida AK. ri (3. 16) 
其 中 ,w 为 用 于 克服 神经 网 络 通 近 误 差 s 的 鲁 棒 项 。 
将 控制 律 式 (3.16) 代 入 式 (3. 12) ,得 
Mr — — (K, - Oir 4AW' g GO +(e +r.) +v — —(K, 十 C)r 十 加 (3. 17) 
K'B.g,-W'g (xz) 十 (Ce tra) tHo. 
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2. 稳定 性 及 收敛 性 分 析 


根据 控制 律 式 (3. 16) 中 是 否 有 w(1) 项 ,e 和 rs 是 否 存在 以 及 神经 网 络 自 适应 律 设计 的 


不 同 , 系 统 的 收敛 性 不 同 。 
D W va) =0, F Ee 和 ru 的 情况 
定义 Lyapunov PRX 
1 


L ——r'Mr 十 Qu ÁCW) (3, 


2 
则 


. EPR" 1 . Ra a 
L—r'Mr Tr Mr 十 tr(W 下 W) 


将 式 (3.17) 代 入 上 式 , 得 


L =—r"K,r + Fr" —20)r + uÑ (F "W--or') --r'(e c) G. 
考虑 机 械 手 特性 ,并 取 
W=—F or 
即 神经 网 络 自 适应 律 为 
W = For (3 
则 
L—-—r'K,rMr'G +r) K-K su lr? esta) lr 
其 中 , le | Kes, | rs ll Sba 
当 满足 下 列 收敛 条 件 时 ,过 0: 
lr ll SS Gu 520/K ais (3. 
2) Ht o) —0,€ —0.7,—0 的 情况 
Lyapunov PK Jj 
L = r° Mr Su F W) (3. 
此 时 控制 律 和 自 适 应 律 为 
r=W'g (x) + Kr (3 
W — For! (3 
由 式 (3. 17) 知 
Mr — —(K, +0)r --W'o (x) 
则 


; 加 
L-—r'Mr Tr Mr--—r'K,rso 


3) Ho G)—0, ff fEe 和 ri, 自 适应 律 采取 UUB 的 形式 
Lyapunov 函数 和 控制 律 取 式 (3.22) 和 式 (3. 23). 
自 适 应 律 为 


W —Fgr' —kF | r | W (3. 


18) 


19) 


. 20) 


21) 


22) 


. 23) 


. 24) 


25) 
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则 根据 式 (3. 19) ,有 
L——r' Kr M20r +W Weg ror (e cc) 
将 式 (3.25) 代 和 人 上 式 , 得 
L ——rK,r--uW'C- pr! -klrlW--or)-4r'G-u) 
——r'K,r +k | rl tw (QW W) +r" Ce 7 0) 
由 于 
tW wW) -— OY,W».— IW ll es IW He IW HL — IW HE 
则 
L«—Kyslrl*oklrl lWI Wa lWlg H enba) llr 
——]|rltXKlrl-£ÀlWlsClWlg—W.2-Gs-454)2 
由 于 
Kus lr | d &IIWICIWIg—Wa-— Guy 5) 
—kCIW ll; —W,./2)* —&WLL/44- Kl rl — Gu 4-50 
则 要 使 工 志 0, 需 要 


kW /4 + (en d bà) 
lr d 人 (3. 26) 


IW le Z Wua/2-- JW, /4-4- (eq 4-04) /k (3. 27) 
4) 存在 e 和 rs, 考 虑 鲁 棒 项 vw(i) 设 计 的 情况 
将 鲁 棒 项 v 设计 为 

v=—(en 十 bu)sgn(r) (3. 28) 
控制 律 取 式 (3. 16) ,神经 网 络 自 适应 律 取 式 (3. 20)。 
定义 Lyapunov 函数 为 
L = 5r Mr ue EOD 
则 


L—r'Mr tyr MeV PW) 
将 (3.17) 式 代入 上 式 , 得 
Le -rK r +r" OL 20) rH WA Fp Wor") +r Ge trs?) 
则 


L——r'K,r--r'( +r; +v) 
由 于 
r! (e t-c,7- v) —r!(Ce t c,0 9r! v—r'(e 十 rz) 一 | 外 re 十 2) 委 0 
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则 
L<0 

针对 以 上 4 种 情况 ,由 于 当 工 二 0 时 ,xr 三 0, 根 据 LaSalle 不 变性 原理 ,闭环 系统 渐进 稳 
定 ,too 时 ,r>0。 由 于 工 写 0, 工 0, 则 LL 有 界 , 从 而 W 有 界 ,但 无 法 保证 W 收敛 于 0。 

3.2.3 针对 f(x) 中 各 项 分 别 进行 神经 网 络 逼 近 

控制 律 为 

r=W'p (x) +K r— v (3. 29) 
f FEX v 取 式 (3. 28) 。 由 式 (3.12) 知 ,被 控 对 象 中 的 f(x) 项 可 写 为 
fG0—MGOE GO -C( 40€ GO) -GCGD FC) 


HEP, g OSa A 656, aS tHe, 
采用 RBF 神经 网 络 ,可 以 对 f Oo rp 85 4 39 4p ETE yr : 


MG) —Wigws(q? 
€( d) —Wig,.QG.d) 
G(q) =Wip (q) 


F(p —Wig,QG) 


则 
Pm 
fir) =W tC) Wea Wi w” (3. 30) 
9 
Pr 
Ọm 
pr T r 1 T T 
其 中 ,gp Ca hem W 一 [Ww We We Wer |]. 
9 
9r 
自 适 应 律 为 
Wy =Fuypur —kguFy |r |l Wy (3. 31) 
W, —Fyoyr! — ky, F. lr | Wy (3. 32) 
Wao — Fagor! — kF |r | W. (3. 33) 
W, =F ppr" = keF: | 厂 | W; CS. 34) 
HB .Eu20. Ry 270, b 20. bL770, 
稳定 性 分 析 如 下 : 


定义 Lyapunov 函数 为 


1 l lx : A 
L—-—r'MrtuSFa Wy) XUOYSFVMAOT 
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Ftr WE PE We) + Ftr (WIPE W) 
则 
L=r Mr Lr! Mr OL ES Wu) He OFLE ZW.) + 
trCWL FG Wu OVI EZW,) 
将 式 (3.17) 代 人 上 式 , 得 


L- —r'K, ror OI - 2V Or +r" (e Hra) +r" v d Wi FaWy 十 purT) 十 


WEES Wy tovr") + WI OS We Hour) --uWICF;W, 十 prrT) — (3.35) 
考虑 机 械 手 特性 ,并 将 神经 网 络 自 适 应 律 式 (3. 3D 一式 (3. 34) 代 人 上 和 式 , 得 
L ——r'K,r-- hy llr || WI Wy Wu) +y l| r ll tw Wy Wy) 十 
bs rt (W, We) d+ Re lr ll ttWE QW; —W or (et+r) tr v 
由 于 


tw (W—W) —QOV,W»,— IWIzs WI e w IL e— IW Dus 
考虑 鲁 棒 项 (3. 28) . Dll] 


L L-K a lb rl? rl lW,lgCOWaus— | Wylg-4 


ky lE rl Wy | OR Vu, — Wy D + 

ko rl We I r Wenms — | We I + ku lr) I Wi I eW em — | Wr Ile 
—— rltKalrl-F 5s Wy Il eC Wy ll e — W mm) 十 

ky I Wy Ie Wy e — Wy) + ke | Wa Ie We Ilr — W anx) + 


Eg I W, I FE( | W; | F -W radd 
由 于 
k IW HL eW I| eW m) =k CW |l &— 
要 使 了 过 0 ,需要 


| T | 二 


127 —kW? an / 4 


max Wa ax 


k MW Max/ 4 T k VW /4 十 ko Wu / 4 i k Was /4 
K vmin 


(3. 36) 


或 
| Wy | F x W say H. | Wy | F < Wx 且 | Ws | F X W Gmnx 且 | Wi | F < M xg 
(3.34) 


XE LOTUN bs 足够 大 , 当 ! 一 ce 时 ,r 一 0, 从 而 e 一 0,E 一 0。 由 于 工 三 0 过 0， 
则 L 有 界 , 从 而 r MWy Wy We Wr 有 界 , 但 无 法 保证 Wu Wy We W KAFE. 
3.2.4 仿真 实例 


选择 二 关节 机 机 械 臂 系统 ,其 动力 学 模型 为 
M(q)0q--V(q.q)0q -G(qD)--F(q)--c,—c 
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其 中 ， 
Pitpst2pscosgs p+ picosq; 
M(g)= | Mis e oPODP "| 
p: + pscosg; P2 
sudes n — ps (di bn 
psdising; 0 
pes Pigcosq, 十 psgcos(q, Hq) 
psgcos(q, 十 gz) 


F(g)=0.2sgn(g), 4 =[0.1sin(t) 0.1sin(t)] 

WO p—[pispesDassbisbsl—L2.9.0. 76,0. 87,3.04,0. 87]。RBF 网 络 高 斯 基 函 数 
参数 的 取 值 对 神经 网 络 控制 的 作用 很 重要 ,如 果 参 数 取 值 不 合适 ,将 使 高 斯 基 函 数 无 法 
得 到 有 效 的 映射 ,从 而 导致 RBF 网 络 无 效 。 故 c 按 网 络 输入 值 的 范围 取 值 , 取 4 二 0. 20. 

[一 1.5 一 1 一 0.5 0 0.5 1 ] 
一 1.5 —1 —0.5 0 0.5 1 
c—0,1X|—1.5 —1 —0.5 0 0.5 1 
.5 —1 —0.5 0 0.5 1 
.5 —1 —06.5 0 0.5 1 


,网 络 的 初始 权 值 取 零 ,网 络 输 入 取 


= =e =e =e LB 
a a a a Ci 


一 1 
z—|e é qa d. dil. 

系统 的 初始 状态 为 [0.09 0 一 0.09 0], 两 个 关节 的 角度 指令 分 别 为 gu 一 0. 1sint， 
da 一 0. 1sint ,控制 参数 取 K,= 二 diag (20.20) ,F 二 diag {1.5,1.5} ,A =diag {5,5) ,在 鲁 棒 项 
中 , 取 sN 王 0. 20,04 一 0. 10。 

采用 Simulink 和 S 函数 进行 控制 系统 的 设计 ,神经 网 络 权 值 矩 阵 中 任意 元 素 初 值 取 0. 10。 
AMA VES ART TRY. chap3 9ctrl. m, 按 3. 2. 2 节 第 4 种 情况 设计 控制 律 ,控制 律 取 式 (3. 16), 
v 取 式 (3. 28) , 自 适应 律 取 式 (3. 20)。 采 用 总 体 允 近 控制 器 ,仿真 结果 如 图 3-3 一 图 3-6 所 示 。 


0.2 1———— —— T T 


position tracking for link 1 
e 


—-0.1 
-0.2 
5 10 15 20 25 30 35 40 
time(s) 
`o o3 
K=] 
S 02 
Em 
ezi 0.1 
Z 
8 
g 9 
& -0.1 1 
0 5 10 15 20 25 30 35 40 
time(s) 


图 3-3 关节 1 及 关节 2 的 角度 跟踪 仿真 结果 


speed tracking for link 1 


speed tracking for link 2 


control input of link 1 


control input of link 2 
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0.5 
0 4 
-0.5 | 
一 
0 5 10 15 20 25 30 35 40 
time(s) 
0.6 
0.4 
0.2 
A 
INN NZ NÉ NN SN 
-0.2 
0 5 10 15 20 25 30 35 40 
time(s) 


3-4 关节 1 及 关节 2 的 角速度 跟踪 仿真 结果 


0 5 10 15 20 25 30 35 40 
time(s) 
20 
10 
0 
-10 
0 5 10 15 20 25 30 35 40 
time(s) 


图 3-5 ”关节 1 及 关节 2 的 控制 输入 仿真 结果 
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图 3-6 关节 1 及 关节 2 的 上 f(x) | Ri | foo | 的 仿真 结果 


仿真 程序 如 下 : 
(1) Simulink 主 程序 : chap3_2sim. mdl。 


(2) 位 置 指令 子 程序 : chap3_2input. m, 


function [sys, x0, str,ts] = spacemodel(t,x, u, flag) 
switch flag, 
case 0, 


[sys, x0, str, ts] = ndlInitializeSizes; 
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case 1, 
sys = ndlDerivatives(t,x,u); 
case 3, 
sys = mdlOutputs(t,x,u); 
case (2,4,9) 
sys-[]; 
otherwise 
error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0, str, ts] = mdlInitializeSizes 


sizes = simsizes; 


sizes.NumContStates 
sizes.NumDiscStates 
sizes.NumOutputs 


sizes.NumInputs 
sizes.DirFeedthrough 


sizes.NumSampleTimes 


LU 
= SAD SDS 
EM 


SyS 7 simsizes(sizes); 


x0 = [] 
str = []; 
ts - [00]; 


function sys = ndlOutputs(t, x, u) 
qdl = 0.1 * sin(t); 

d qdl=0.1*cos(t); 

dd gqdl= —0.1* sin(t); 

qd2 = 0.1 * sin(t); 

d qd2 7 0.1* cos(t); 

dd qd2= -0.1* sin(t); 


sys(1) = adl; 
sys(2) =d qdi; 
sys(3) = dd adl; 
sys(4) = qd2; 
sys(5) = d qd2; 
sys(6) = dd qd2; 


(3) dà pis E d as f EJF : chap3 2ctrl. m, 


function [sys,x0,str,ts] = spacemodel(t, x, u, flag) 
switch flag, 
case 0, 
[sys, x0, str, ts] = mdlInitializeSizes; 
case 1, 
sys = ndlDerivatives(t,x,u); 
case 3, 
sys = ndlOutputs(t,x,u); 
case {2,4,9} . 
sys-[]; 
otherwise 
error(['Unhandled flag = ',num2str(flag)]); 
end 
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fl 


function [sys,x0,str,ts] = mdlInitializeSizes 

global node c b Fai 

node = 7; 

&m0.1*[-—1.5 —-1—0.500.51 1.5; 
—1,5—4 —0.5/00.5 I 1:5; 
—€1.5—-1 —60.800,.5 1 1.5; 
—-1.55-1.20:5:)91:5)1 L55 
—1.58 —1.—0.5:000.5 1 1.5]; 

b= 10; 

Fai-5* eye(2); 


sizes - simsizes; 
sizes.NumContStates = 2 * node; 
sizes.NumDiscStates  - 0; 
sizes.NumOutputs = 3; 
sizes.NumInputs = 11; 


sizes.DirFeedthrough = 1; 
sizes.NumSampleTimes - 0; 


Sys = simsizes(sizes); 


x0 = 0.1*ones(1,2 * node); 
str = []; 
ts = [I] 


function sys = ndlDerivatives(t, x,u) 
global node c b Fai 

qdl = u(1); 

d adi = u(2); 

dd qdl = u(3); 

qd2 = u(4); 


q= [ql;q2]; 


el = dadl - ql; 

e2 = qd2 - q2; 

del = d qdl-d ql; 
de2 = d qd2- d q2; 
e-[el;e2]; 

de = [del1;de2]; 
r= de + Fai * e; 


qd = [qdl;qd2]; 

dqd = [d_qd1;d_qd2]; 
dar = dqd + Fai * e; 

ddqd = [dd qdi;dd qd2]; 
ddar = ddqd + Fai * de; 
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zl1=[e(1);de(1);qd(1);dqd(1);ddad(1)]; 
z2 - [e(2);de(2);qd(2);dqd(2);ddqd(2)]; 
for j= 1:1:node 
h1(j) = exp( - norn(z1 - c(:, 3))^2/(b* b)); 
h2(3) = exp( - norn(z2 - c(:, 3))^2/(b * b)); 
end 


F=1.5* eye(node); 


fori-1:1:node 
sys(i)-1.5*hl(i)*r(1); 
sys(i+ node) - 1.5 * h2(i) * r(2); 
end 


function sys = mdlOutputs(t, x, u) 
global node c b Fai 


adi - u(1); 
d qdi = u(2); 
dd adl = u(3); 
qd2 = u(4); 
d qd2 = u(5); 
dd qd2 = u(6); 


ql -u(7); 
d q17u(8); 
q2 - u(9); 
d q27u(10); 


q= [ql;q2]; 


el=qdl— ql; 

e2 = qd2 - q2; 
del=d qdi-d ql; 
de2 = d qd2- d a2; 
e= [el;e2]; 

de = [del;de2]; 
r-de*tFai*e; 


ad - [qdl;qd2]; 

dad - [d adi;d qd2]; 
dar = dqd + Fai*e; 

ddqd = [dd qdl;dd qd2]; 
ddqr = ddqd + Fai * de; 


z= [e;de;ad;dqd; ddqd] ; 
W f1-[x(1:node)]'; 
W £27 [x(node + 1:node * 2)]'; 


z1- [e(1);de(1);ad(1);dad(1);ddad(1)]; 
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z2=[e(2);de(2);qd(2);dqd(2);ddqd(2)]; 

for j= 1:1:node 
h1(j) = exp( - norm(z1- c(:,3))^2/(b* b)); 
h2(j) = exp( - norm(z2- c(:,3))^2/(b* b)) 


end 


fn-[W f1*hl'; 
W £2* h2']; 
Kv = 20 * eye(2) ; 


epN = 0. 20; 

bd= 0.1; 

v= - (epN * bd) * sign(r); 
tol-fn*Kv*r-v; 


fn norm- norn( fn); 


sys(1) = tol(1); 
sys(2) = to1(2); 
Sys(3) » fn norm; 


(4) 被 控 对 象 子 程序 : chap3 2plant. m, 


function [sys, x0, str,ts] =s_ function(t, x,u, f lag) 


switch flag, 
case 0, 
[sys, x0, str, ts] = mdlInitializeSizes; 
case 1, 
sys = ndlDerivatives(t,x,u); 
case 3, 
sys = mdlOutputs(t, x,u); 
case (2, 4, 9 ) 


sys = []; 
otherwise 
error(['Unhandled flag = ',num2str(flag)]); 


end 


function [sys, x0, str, ts] = ndlInitializeSizes 


global p g 

sizes = simsizes; 
sizes.NumContStates  - 4; 
Sizes.NumDiscStates = 0; 
sizes. NumOutputs = 5; 
sizes. NumInputs = 3; 


sizes. DirFeedthrough = 0; 


Ii 


sizes. NumSampleTimes 0; 
sys = simsizes(sizes); 
x0-[0.09 0 -0.09 0]; 
str-[]; 

ts-[]; 


p^ [2.9 0.76 0.87 3.04 0.87]; 
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g-79.8; 
function sys = mdlDerivatives(t, x, u) 
global p g 


M7 [p(1) * p(2) * 2 * p(3) * cos(x(3)) p(2) + p(3) * cos(x(3)); 
p(2) * p(3) * cos(x(3)) p(2)]; 

V-[-p(3) * x(4) * sin(x(3)) —-p(3) * (x(2) * x(4)) * sin(x(3)); 
p(3) *x(2) * sin(x(3)) 0]; 

G-[p(4) * a* cos(x(1)) * p(5) * g* cos(x(1) * x(3)); 
p(5) *g* cos(x(1) * x(3))]; 

dq= [x(2);x(4)]; 

F-0.2* sign(dq); 

told- [0.1* sin(t);0.1* sin(t)]; 


tol =u(1:2); 


S= inv(M) * (tol - V * dg- G- F- told); 


sys(1) = x(2); 

sys(2) = S(1); 

sys(3) = x(4); 

sys(4) - S(2); 

function sys = mdlOutputs(t, x, u) 

global p g 

M- [p(1) * p(2) *2* p(3) * cos(x(3)) p(2) + p(3) * cos(x(3)); 
p(2) + p(3) * cos(x(3)) p(2)]; 

V-[-p(3) * x(4) * sin(x(3)) -p(3) * (x(2) * x(4)) * sin(x(3)); 
p(3)*x(2) *sin(x(3)) 0]; 

G- [p(4) *a * cos(x(1)) * p(5) *g* cos(x(1) + x(3)); 
p(5) *g* cos(x(1) + x(3))]; 

dq= [x(2);x(4)]; 

F-70.2* sign(dq); 

told= [0.1¥ sin(t);0.1* sin(t)]; 


qdl = sin(t); 

d qdl = cos(t); 

dd qdl = - sin(t); 
qd2 = sin(t); 

d qd2 = cos(t) ; 

dd qd2 = - sin(t); 

qd1 = 0.1 * sin(t); 
d_qd1 = 0.1 * cos(t); 
dd_qd1 = -0.1* sin(t); 
qd2 = 0. 1 *# sin(t); 
d_qd2 = 0. 1 * cos(t); 
dd_qd2 = - 0.1 * sin(t); 


ql 7 x(1); 
d qi - dq(1); 
q2 7 x(3); 
d q2 = dq(2); 
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q= [a1;q2]; 

el = qdi - q1; 

e2 7 qd2 - q2; 

del = d qdl-d ql; 
de2 = d qd2- d_q2; 

e= [el;e2]; 

de= [de1;de2]; 

Fai =5 * eye(2); 

dad - [d_qd1;d_qd2]; 
dqr = dad + Fai*e; 
ddad = [dd qd1;dd qd2]; 
ddqr = ddqd + Fai * de; 
f=Mxddqr+Vxdqrt+G+F; 


f_norm = norm(f); 


sys(1) » x(1); 
sys(2) = x(2); 
sys(3) = x(3); 
sys(4) = x(4); 
sys(5) = f norm; 


(5) 绘图 子 程序 : chap3, 2plot. m, 


close all; 


figure(1); 

subplot(211); 

pdot(t,xl:,1),'r"bx1(:,2),'b'); 
xlabel('time(s)');ylabel('position tracking for link 1'); 
subplot(212); 

piot(t,x2(:,1),'r',t,x2(:,2), 'b*); 

xlabel('time(s)'); ylabel('position tracking for link 2'); 


figure(2); 

subplot(211); 

plot(t,dxi(:,1),'r',t,dx1(:,2), 'b'); 
xlabel('time(s)');ylabel('speed tracking for link 1"); 
subplot(212); 

plot(t,dx2(:,1),"r",t,dx2(4,2), 'b'); 
xlabel('time(s)');ylabel('speed tracking for link 2"); 


figure(3); 

subplot(211); 

plót(t,toll(:,lL),"e'); 
xlabel('time(s)');ylabel('control input of link 1'); 
subplot(212); 

plot(t,tol2(:,1), r'); 
xlabel('time(s)');ylabel('control input of link 2"); 


figure(4); 
plot(t,f( £d.) '* ic, £( S uA). pr); 
xlabel('time(s)');ylabel('f and fn'); 
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CHAPTER 4 


4.1 单 力 璧 机械手 直接 自 适 应 模糊 控制 


直接 模糊 自 适 应 控制 和 间接 自 适应 模糊 控制 所 采用 的 规则 形式 不 同 。 间 接 自 适 应 模糊 
控制 利用 的 是 被 控 对 象 的 知识 ,而 直接 模糊 自 适应 控制 采用 的 是 控制 知识 。 


4.1.1 问题 描述 


考虑 如 下 方程 所 描述 en md 
fr) bu (4.1) 


y = (4.2) 
其 中 ,f 为 未 知 函数 ,6b 为 未 知 的 正常 数 。 
直接 自 适 应 模糊 控制 采用 下 面 IF-THEN 模糊 规则 来 描述 控制 知识 : 
wW r, Æ Pi H- Hr, Æ PM) u ER (4.3) 
其 中 ,P:;Q" AR PEBS.Hr-—1.2.-.L,.i—10.2. ys 
设 位 置 指令 为 Ym 
e=yn Y =y r, ee 一 (eee ? )! (4. 4) 
选择 KK 二 (k,，…,k1)' EAE stus ?十 … 十 k, 的 所 有 根 都 在 复 平面 左 半 平 面 上 。 
取 控 制 律 为 


u^ = [= f(x) y? K'e] (4. 5) 
将 式 (4.5) 代 入 式 (4.1) ,得 到 闭环 控制 系统 的 方程 为 
e" 4 bue p b,e 0 (4. 6) 


通过 天 BEC. EM roof Le GO) 70. BL BER dr y 渐进 地 收敛 于 理想 输出 yw。 
直接 型 模糊 自 适 应 控制 是 基于 模糊 系统 设计 一 个 反馈 控制 器 v = u Gc 8 ) 和 一 个 调整 
参数 向 量 9 的 自 适应 律 ,使 得 系统 输出 y 尽 可 能 地 跟踪 理想 输出 y， 


4.1.2 模糊 控制 器 的 设计 


直接 自 适应 模糊 控制 如 为 
u —up(x | (4. 7) 
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其 中 ,wb 是 一 个 模糊 系统 ,6 是 可 调 参数 集合 。 
根据 模糊 系统 万 能 逼近 定理 ,模糊 系统 uo 可 由 以 下 两 步 来 构造 ”: 
(1) 对 变量 x G — 1,2, 20 GE Xm, EMRA A, OT 1,2 m; 


D 用 以 下 [T w 条 模糊 规则 来 构造 模糊 系统 w(x10 )， 


IF z; is A}! AND…AND x, is A”, THENujisS" " (4. 8) 
Rp, 1.2, m;; 11,2, 
采用 乘积 推理 机 . 单 值 模糊 器 和 中 心平 均 解 模糊 器 设计 模糊 控制 器 , 即 
»E UD "(Tek Go) 
ust [103 2 (4. 9) 


Sos P Fab dua) 
=1 =l) is i 


IL», 1 l 
令 记 "是 自由 参数 ,分 别 放 在 集合 g ERT 中 ,ua Cr 为 变量 了 ,属于 模糊 集合 A， 的 隶 
属 函 数 , 则 模糊 控制 器 为 
u—uy(x |0)=0"e (x) (4. 10) 


其 中 ,6 oo [| m, Hn tt H8 lase, 个 元 素 为 
TI». Cr;) 
i=1 ^j 


2M Hey Cx J 
模糊 控制 规则 式 (4. 3) 是 通过 设置 其 初始 参数 而 被 伐 入 模糊 控制 器 中 的 。 
采用 模糊 系统 实现 u^ muy wh ,控制 律 为 


n (x) == 


U5 =u" tw 
其 中 ,为 逼近 误差 ,zw 有 界 。 
则 
ü-—ug-—u'-d-py-—u') (4. 11) 


4.1.3 自 适 应 律 的 设计 
将 式 (4.5). 式 (4.7) 代 入 式 (4.1) ,并 整理 得 


e" ——K'e--b[u' —uy(x 10)] (4. 12) 
e 
| 0 1 0 0 0 0 [0] 
0 0 1 0 0 0 0 
AS ; LR Y ^oi £o. =l? (4. 13) 
0 0 0 0 0 1 0 
—k = — hk; | b 
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则 闭环 系统 动态 方程 式 (4. 12) 可 写成 向 量 形式 


é —Ae-d-b[u' —up(x |8)] (4. 14) 
定义 最 优 参数 为 
0 —arg min | sup | uis Gx EPLIM |] (4. 15) 
iL»; r€R" 
ger T! 
XE XB NB EV 2578 
w-—up(x|0')—u' (4. 16) 
由 式 (4. 14) 可 得 
é —Ae-Ftb(up(x | 0') —ug(x |00)) —b(upg(x | 0) —u^) (4. 17) 
由 式 (4. 10) ,可 将 误差 方程 式 (4. 17) 改 写 为 
é=Ae+b(0" —0)"E(x)—bw (4. 18) 
定义 Lyapunov 函数 为 
—lep,. E —9 yrqg* —8) (4. 19) 
2 2Y 
其 中 ,参数 y 是 正 的 常数 。 
P 为 一 个 正定 矩阵 且 满 足 Lyapunov 方程 
ATP 4- PA —— (4. 20) 


HR'B.Q 是 一 个 任意 的 n Xn 正定 矩阵 ,A 由 式 (4. 13) 给 出 。 


1 b ; 
RV,—. e Pe V= 5" —0)!(80' —005,^ M—b(8' —08)'& (x) — bw, Dll 


式 (4. 18) 变 为 
e —Ae-4- M 


V, 一 了 7Pe 十 了 erPe 一 了 (erA I 十 MI)Pe 十 je Pa e 4- M) 


一 je (ATP -- P ADe + ;M'Pe 十 je PM 


=— 5 e"Q e d M" Pe + e! PM) —— 3€ Qe-F e'PM 


即 
V, =- eQ e+e Pb —8)"6 (3) —w) 
P$ ==" gy 
y 
V 的 导数 为 
V-——e'Qe-ePb[G —0)'&£(x)—w] E 8 )70 (4. 21) 
4 p, 为 P 的 最 后 一 列 , 由 5b 一 [0，…，0，bJ' n[Ale'Pb-e'p,b., Wis C4. 21) 变 为 
V= T | 0 0 )'[ye! p,£ (x) —0] — e! p,bw (4. 22) 


取 自 适应 律 


Ó — ye! p, (x) (4. 23) 
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则 
V.—— eQ e— e p,bu (4. 24) 


TEZUESQERAS VS D MODESRIZAIULIITLISNODLDL ES. 
分 小 ,并 满足 |erp,bno | e Q e E UE VCO, RRA EE. 


根据 LaSalle 不 变性 原理 , 当 V=0 RE. — re Qe — e" p, bu, v 充分 小 ,1 一 co 时 ， 


e 一 0。 系 统 的 收敛 速度 取决 于 Q@。 
由 于 V 宇 0,V 志 0, 则 VV 有 界 , 因 此 e 和 6 有 界 , 但 无 法 保证 9 收敛 于 09” 。 
直接 型 自 适应 模糊 控制 系统 的 结构 如 图 4-1 所 示 。 


被 控 对 象 
x) — f (x) bu, y a x 


模糊 控制 器 
up(x|6) - 0 £x) 


自 适 应 律 
0 — Ye' p,&(x) 


4-1 直接 型 自 适 应 模糊 控制 系统 的 结构 


4.1.4 仿真 实例 
被 控 对 象 为 单 力 臂 机 械 手 : 
5 一 一 了 (db 十 mglcosg) 十 于 (r 一 上 ) 


初始 值 (0) 


其 中 ,ra 为 摩擦 模型 。 

摩擦 模型 为 库仑 摩擦 十 黏 性 摩擦 模型 , 即 

r,—sgn(Ó (1)) Cki | G) | +k) 

其 中 ,， 和 ke 为 正 的 常数 。 

角度 指令 为 x,(t) 二 sin(xt)。 取 以 下 6 种 隶属 函数 : wNvs(z) 一 1/(1 十 exp(5(Zz 十 2)))， 
Ey GE) —expC— Ge- 1.5)! ) pm (x) = expC— (x 4-0. 5)! ) n, (n) —exp(—(—0.5)*), 
Bg (x) —expC—G —1.5)*) ,pp Gr) —1/(LexpC—5r —2))), 

系统 初始 状态 为 L1,0],0 的 初始 值 取 0, 采 用 控制 律 (4. 100 . 自 适 应 律 取 式 (4. 23) , 按 
式 (4.20) 求 P, 取 @= » M sk =1,k:=10, B3 ASOBC y —100, 


根据 隶属 函数 设计 程序 ,可 得 到 隶属 函数 ,如 图 4-2 所 示 。 在 控制 系统 仿真 程序 中 ,分 
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3| H] FS,.FS, 和 FS 表示 模糊 系统 & (x ) 的 分 子 、 分 母 及 & (x), 仿 真 结 果 如 图 4-3 和 图 4-4 


所 示 。 
1 
0.9 
$ 0.8 
Ey 
©% 07 
S 
S 0.6 
£e 
rz: 
,g. 0.5 
£ 
B oa 
E 
2 03 
0.2 
p ANC 
i E AE a 
-3 
图 4-2 x 的 隶属 函数 
1.5 
1 
eo 0.5 
| 
5 
E o 
g 
Q 
$ -0.5 
一 | 
-1.5 
5 10 15 20 25 30 35 40 45 50 
time(s) 
4-3 角度 跟踪 
仿真 程序 如 下 : 
d) 隶属 函数 设计 : chap4 1mf. m. 
clear all; 
close all; 
L12 - 3; 
L2=3; 
L=L2- L1; 
T=0.001; 


x= L1:T:L2; 


12 
lor 
8 
6 
5 
B 4 
- 
= 2 
S 
| 
一 2 
对 | 
-6 1 1 
0 S 10 15 20 25 30 35 40 45 50 
time(s) 
图 4-4 控制 输入 信号 
figure(1); 
for i=1:1:6 


if i==1 
u=1./(1+exp(5* (x+ 2))); 
elseif i==6 
u-z1./(1*texp(-5* (x-2))); 
else 
u-exp(- (x*2.5— (1-1)).^2); 
end 
hold on; 
plot(x,u); 
end 


xlabel('x'); ylabel( membership function degree'); 


(2) Simulink 主 程序 : chap4. 1sim. mdl。 


To Workspaceb 


Chap4 1plant 


Sine Wave 
Chap4 1ctrl 


Derivative 


Clock To Workspace 


(3) 控制 器 S 男 数 : chap4 lctrl. m. 


function [sys, x0, str,ts] = spacemodel(t, x, u, flag) 
switch flag, 


case 0, 
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[ sys, x0, str, ts] = ndlInitializeSizes; 
case 1, 

sys = ndlDerivatives(t,x,u); 
case 3, 

sys = ndlOutputs(t,x,u); 
case (2,4,9) 

sys- []; 
otherwise 

error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0,str,ts]  mdllInitializeSizes 
sizes - simsizes; 
sizes.NumContStates 


外 
Ww 
a 
T. 


sizes.NumDiscStates 


LU 
O he AHO 
`e 


sizes. NumOutputs 
sizes.NumInputs 


sizes.DirFeedthrough 


sizes.NumSampleTimes 
SyS 7 simsizes(sizes); 

x0 [zeros(36,1)]; 

str = []; 

ts z Ip 

function sys = mdlDerivatives(t, x,u) 
r-u(1); 

dr = u(2); 

xi(1)2u(3); 

xi(2) = u(4); 


e-r-xi(1); 
de= dr— xi(2); 


gama = 100; 


k271; 
k1 = 10; 
E= [e,de]'; 
A= [0 -k2; 
3. ejes 
Q= [150 0;0 150]; 
P= lyap(A,0) ; 
LELLLLLLLLELLLLLLLLELLELELELLLLELL 
FS1 = 0; 


ul(1)=1/(1 + exp(5* (xi(1) + 2))); 
ul(6) =1/(1+exp( -5* (xi(1)-2))); 
for i= 2:1:5 
ul(i)-exp(- (xi(1) *1.5- (i-2))^2); 
end 


u2(1) 7» 1/(1 + exp(5 * (xi(2) * 2))); 
u2(6) = 1/(1*exp( - 5* (xi(2) - 2))); 


fori-22:1:5 
dali) = expl- (xi(2)-t1.5- (1-2))^2); 
end 


fori-21:1:6 
for j=1:1:6 
FS2(6* (1-1) *j)-ul(i)*u2(j); 
FS1 = FS1 + ul(i) * u2(j); 
end 
end 
FS = FS2/FS1; 


b- [0;1]; 
S= gama * E' * P(:,2) * FS; 


fori-21:1:36 
sys(i)-S(i); 
end 


function sys = mdlOutputs(t, x, u) 


r-u(1); 
dr-u(2); 
xi(1) -u(3); 
xi(2) = u(4); 


for iz1:1:36 
thtau(i,1) 7 x(i); 
end 


FS1 = 0; 
u1(1)=1/(1 * exp(5 * (xi(1) + 2))); 
u1(6)=1/(1+exp( -5* (xi(1)-2))); 
fori-22:1:5 

ul(i)* exp(- (xi(1)*1.5- (i-2))^2); 
end 


u2(1) » 1/(1 + exp(5 * (xi(2) * 2))); 
u2(6) 2 1/(1* exp( -5* (xi(2) - 2))); 
fori-22:1:5 

u2(i)*exp( = (xi(2)f1.5-(1-2))^2); 
end 


fori-1:1:6 
for j21:1:6 
FS2(6* (i-1)*j)-2ul(i) *u2(j); 
FS1 = FS1 -*ul(i)*u2(j); 
end 
end 
FS - FS2/FS1; 


xax CEPS EBENE ce 49 
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ut = thtau' * FS'; 
sys(1) = ut; 


(4) 被 控 对 象 S PR: chap4 lplant. m, 


% S — function for continuous state equation 


function [sys,x0,str,ts] = s function(t, x, u, f lag) 


switch flag, 
% Initialization 
case 0, 
[ sys, x0, str, ts] = mdlInitializeSizes; 
case 1, 
sys = mdlDerivatives(t, x,u); 
% Outputs 
case 3, 
sys = mdlOutputs(t, x,u); 
% Unhandled flags 
case (2, 4, 9] 
sys = []; 
% Unexpected flags 
otherwise 
error(['Unhandled flag = ',num2str(flag)]); 


end 


function [ sys, x0, str, ts] = mdlInitializeSizes 


sizes = simsizes; 


sizes. NumContStates 


sizes. NumDiscStates 


sizes. NumOutputs 


sizes.NumInputs 


LU 
O O PNON 
S 


sizes.DirFeedthrough 


LU 


sizes. NumSampleTimes 


Sys = simsizes( sizes); 

x0 = [0.15 0]; 

stre]: 

ts-[]; 

function sys = mdlDerivatives(t,x,u) 
g79.8; 

m-1; 

120.25; 

d= 2.0; 

I-4/3*m*1^2; 


tol = u; 
k1 =0, 303 
k2 = 1.53 


told = sign(x(2)) * (kl * abs(x(2)) + k2); 
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fx-1/I*(-d*x(2)-m*g*1*cos(x(1))); 
gx= 1/1; 


sys(1) = x(2); 

sys(2) » fx * gx * (tol- told); 
function sys = mdlOutputs(t, x,u) 
sys(1) - x(1); 

sys(2) - x(2); 


(5) 作 图 程序 : chap4. 1 plot, 


close all; 


figure(1); 
plot(t,y(*,1), "t'y, (2,2), by 
xlabel('time(s)'); ylabel('position tracking!'); 


figure(2); 
plot(t,u(:,1), 'r'); 
xlabel('time(s)');ylabel('control input'); 


4.2 单 力 璧 机 械 手 间接 自 适 应 模糊 控制 


模糊 控制 器 的 设计 不 依靠 被 控 对 象 的 模型 ,但 它 却 非 常 依 靠 控制 专家 或 操作 者 的 经 验 
知识 。 模 糊 控 制 的 突出 优点 是 能 够 比较 容易 地 将 人 的 控制 经 验 融 人 控制 器 中 ,但 大 缺 乏 这 
样 的 控制 经 验 ,很 难 设 计 出 高 水 平 的 模糊 控制 器 。 而 且 , 由 于 模糊 控制 器 采用 了 IF-THEN 
控制 规则 ,不 便于 控制 参数 的 学 习 和 调整 ,使 得 构造 具有 自 适 应 的 模糊 控制 咒 较 困难 。 

自 适应 模糊 控制 有 两 种 不 同 的 形式 : 一 种 是 直接 自 适应 模糊 控制 , 即 根据 实际 系统 性 
能 与 理想 性 能 之 间 的 偏差 ,通过 一 定 的 方法 来 直接 调整 控制 器 的 参数 ; 男 一 种 是 间接 自 适 
应 模糊 控制 , 即 通过 在 线 辨 识 获 得 控制 对 象 的 模型 ,然后 根据 所 得 模型 在 线 设计 模糊 控 
Tl AE o 


4.2.1 问题 描述 
考虑 如 下 n 阶 非 线性 系统 : 


和 (4. 25) 
其 中 ,f 和 4g 为 未 知 非 线性 函数 ,wxER"” A y € R" 分 别 为 系统 的 输入 和 输出 。 
设 位 置 指令 为 y,. 令 
e—ya——9.—£. e=le, ë e e") (4. 26) 
选择 下 二 (k,,…,k1)" ,使 多 项 式 s" 十 his” 十 … 十 k, 的 所 有 根部 都 在 复 平面 左 半 平 
HE. 
取 控 制 律 为 
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u = f(x)+ y? -K'e] (4. 27) 
将 式 (4. 27) 代 入 式 (4. 25) ,得 到 闭环 控制 系统 的 方程 
e" pykje"? +“ 二 ke =0 (4. 28) 
由 K 的 选取 ,可 得 上 ~ce 时 e() 一 0, 即 系统 的 输出 y 渐进 地 收敛 于 理想 输出 m o 
来 消除 其 非 线性 的 性 质 


如 果 非 线性 函数 f(x) 和 g(x) 是 已 知 的 , 则 可 以 选择 控制 u 
然后 再 根据 线性 控制 理论 设计 控制 器 。 

4.2.2 自 适 应 模糊 滑 模 控制 器 的 设计 

如 果 SOOM g (xz) 未 知 ,控制 律 式 (4. 27) 很 难 实现 。 可 采用 模糊 系统 F(x) 和 (x) 代 


Z f(x) 和 g (x), 实现 自 适应 模糊 控制 。 


1. 基本 的 模糊 系统 
以 f(x191) 来 通 近 f(x) 为 例 , 可 用 以 下 两 步 构造 模糊 系统 (x 191) 5 ， 


(19 对 变量 z;(—152,,n),$E8 X. bi 个 模糊 集合 A' (0 a a pdg 


采用 以 下 [[ 条 模糊 规则 来 构造 模糊 系统 S l0): 


(2) 
and z, is Ar THEN fis E^" 


R; IF z, is A; and … 


HEP, =l, 2$ seipi 一 ]， p M ** No 
采用 乘积 推理 机 、 单 值 模糊 器 和 中 心平 均 解 模糊 器 , 则 模糊 系统 的 输出 为 


HON ( Hey (x) ) 
(4. 30) 


451 E =I 


f(x | 00 = pi 


其 中 ,py Cxi) H x; 的 隶属 函数 。 


y? LETT 


f | 60 —6076 (x) 


其 中 , (xz) 为 T[ p 维 向 量 ,其 第 el, 个 元 素 为 


2. 自 适 应 模糊 滑 模 控制 器 的 设计 
采用 模糊 系统 通 近 f Hg. Pd 27) 2E 24 
[一 fa | 0D +y” +K] 'e] 


EE 


fa 160—618 GO. £G 16) —61g GO 
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其 中 ,6 (x ) 为 模糊 向 量 , 参 数 0/ 和 02 根据 自 适 应 律 而 变化 。 
设计 自 适 应 律 为 
0, — — y,e! P bê (x) (4. 35) 


0, — — 7:6" P bg (Xu (4. 36) 
其 中 ,7 OO WAFER g 的 模糊 问 量 。 
自 适 应 模糊 控制 系统 如 图 4-5 Bron. 


被 控 对 象 
x? = f(x) - g(x)u, y=x 


模糊 控制 器 


[-/ (xo) + K'e] 


1 
&(xle,) 


自 适 应 律 
0, - —y,e' Pbé(x) 
0, - —y,e' Pby(x)u 


&,(0) 0.(0) 


图 4-5 自 适应 模糊 控制 系统 


4.2.3 稳定 性 分 析 
由 式 (4.33) 代 入 式 (4.25) 可 得 如 下 模糊 控制 系统 的 闭环 动态 


e™ ——Kle--Lf(x |0) — f] +E 10) — gau (4. 37) 
^ 
o 1 0 0 0 0] 01 
0 0 1 0 0 0 0 
A=] : : MER OG A. 2 1s Reels (4. 38) 
0 0 — 8 Q - íj 0 
|. = bi =k] Al 
则 动态 方程 式 (4. 37) 可 写 为 向 量 形式 
é-Ae- bif 100— fG»--G 100 —g(GDu) (4. 39) 
设 最 优 参数 为 


0/ —argmin |sup PC (8) — f(x) | (4. 40) 
OEA Ler", 

0; —arg min sup £s D (4.41) 
$,€0, | rer 


其 中 ,2 flQ, 151750, 和 09, 的 集合 。 
定义 最 小 逼近 误差 为 
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w= fC l0 — fer) Gc 1012 —g(xDu (4. 42) 
式 (4.39) 可 写 为 


é—Ae bif Gc [00 —füx 10/5] -4-L£(Gx 10, —g(x | 0:2 ]u +w} (4. 43) 
将 式 (4. 34) 代 入 式 (4. 43) ,可 得 闭环 动态 方程 
é=Ae+tb[(0.—0r)'€(x)+ (00, —0°) auw] (4. 44) 
该 方程 清晰 地 描述 了 跟踪 误差 和 控制 参数 9, RIO, 之 间 的 关系 。 自 适应 律 的 任务 是 为 9. 
和 90, 确定 一 个 调节 机 理 , 使 得 跟踪 误差 e 和 参数 误差 9. 一 9; 和 90, 一 09; 达到 最 小 。 
定义 Lyapunov 函数 为 


E oa. 1 M . 1 - 
Sa | —-— Bc T ) -— , 45 
V — ge Pe +z (8, — 07 )* (0, —6, ) gy; (0.76: 0, — 0; ) (4.45) 
其 中 ,yi oy, 是 正常 数 ,P 为 一 个 正定 矩阵 且 满 足 Lyapunov 方程 
ATP +PA =— (4. 46) 
其 中 ,O 是 一 个 任意 的 n Xn 正定 矩阵 ,A 由 式 (4. 38) 给 出 。 
tus nl eg ritea —a? -l eg tyto —p* 
取 V, — e" Pe V, gy, 6-81 7 0,—8/).V,— 5-0, —8; V 0, —0; 5. 
4 M —b[(8,—0/ E (x) - (0, —0; ) 9 GOu rw ]. ls C4. 44) 变 为 
é —Ae-c-M 
V, — 56 Pe + e! Pé — 7 (^ A -MOPe ze PU e +M) 
] T T l r l T 
=—e (A P+4PA)e+—M Pe e PM 
2 2 2 
= 一 je Qe | 7 (M' Pe +e"PM) =— Te'Qe te PM 
即 
V, =—7e Qete Pbwt (0, — 0] "eP bE (x) + (0, —0; e Pb Gu 
, 1 sos 
Vs c EA =e ) Å, 
1 
: 1 
V, = 一 (0. — 0; )"À, 
Y» 
V 的 导数 为 
V —V, cV, Ho — Be Q ee P bw 7-0, — 0; Y (B + ye'P be GO] + 
1 
y. 0, —0; 0, + y, e P by Cx)u] (4. 47) 


将 自 适 应 律 式 (4. 35) 和 式 (4. 36) 代 入 上 式 , 得 


5 
V=—5e Qete Pbw (4. 48) 


HF- e'o e O0 , 3l iE E ERE w JEW RAE TTEA V —0, 
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由 于 Q0. 是 最 小 通 近 误差 ,通过 设计 足够 多 规则 的 模糊 系统 ,可 使 刀 充分 小 ,并 满 
E Je" Pow | 过 了 e"Qe, 从 而 使 得 V0, 闭 环 系统 为 渐进 稳定 。 


根据 LaSalle 不 变性 原理 , 当 V 二 0 时 ,一 Fe'Qete'Pp bw 三 0, 当 w 充分 小 ,1 一 oo 时 ， 
e 习 0。 系统 的 收敛 速度 取决 于 Q. 

由 于 V 宇 0,V 志 0, 则 V 4i 9t. AIE e.0, MO, 有 界 , 但 无 法 保证 9, 和 6, KAFO 6; 
即 无 法 保证 f Cr RI g Ceo EH if T 

4.2.4 仿真 实例 

被 控 对 象 为 一 单 力 臂 机 械 手 : 

5 一 一 了 (db +mgleosg) 十 了 (1.0 二 0 二 6) 

其 中 ,ru 为 摩擦 模型 。 

Hm —1.1—0. 25.4—2.0,1— ml, 角度 指令 为 Yn) =0. lsin). EFX ECx) AI 


7(z), 取 以 下 5 种 隶属 函数 : jv Cr = exp[ — (Cr; + x/60/€(n/240)! ] p Cx; = 
exp[ — (C(x; 4-2/12)/€(x/24))* ], p, Gr) — exp[ — Ge; / /24))! J ,pps Gr; ) — expL 7 (Gr; — 
x/12)/(x/24))* ]. ppu Gr) — exp[ — Gr; —2/6)/€(x/24))* ], WAF f 和 gg 的 模糊 规 
则 分 别 有 25 条 。 

根据 隶属 也 数 设计 程序 ,可 得 到 隶属 函数 图 ,如 图 4-6 所 示 。 


membership function degree 


图 4-6 隶属 函数 
机 械 手 初始 状态 为 L0. 15.0].0, RIO, 的 初始 值 取 0. 10, 采 用 控制 律 式 (4. 33) , 按 式 (4. 46) 


10 0 


RPE- p 10 


| ,Ri 一 2,A: 王 1, 自 适应 参数 取 y, =10,y7: —1. Á EMERARA. 35) A 


式 (4. 36)。 
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在 程序 中 ,分 别 用 FS, FS, 和 FS 表示 模糊 系统 & (x ) 的 分 子 、 分 母 及 86 (x) ,仿真 结果 如 
图 4-7 一 图 4-10 所 示 。 


0.3 


angle tracking 
e 


un 
一 
— 
Un 
N 
© 
N 
un 
Ww 
eo 


o 
小 


angle Speed tracking 
b b o 
D N e N 
H 


un 
Us 
So 


10 15 20 25 
time(s) 
4-7 角度 及 角速度 跟踪 


control input 
N 


0 5 10 15 20 25 3 
time(s) 


图 4-8 控制 输入 信号 
间接 模糊 自 适应 控制 仿真 程序 有 5 个 。 


仿真 程序 如 下 : 
(1) 隶属 函数 设计 : chap4_2mf. m, 


© 


clear all; 


close all; 


LI = - pi/6; 


fx and estiamted fx 


gx and estimated gx 


L2 - pi/6; 
L-I2-Ll; 


T-L* 1/1000; 
x-7L1:T:l2; 


figure(1); 
for i=1:1:5 
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5 10 15 


time(s) 


20 


图 4-9 fG.D 及 f(x.1) 的 变化 


10 15 


time(s) 


20 


图 4-10 gx ORE DEL 


gs= —[(x* pi/6 - (i- 1) * pi/12)/(pi/24)].^2; 


u- exp(gs); 


hold on; 


plot(x,u); 


end 


xlabel('x');ylabel( membership function degree'); 
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(2) Simulink 主 程序 : chap4_2sim. mdl。 


Sine Wave 


To Workspace10 


Mux3 To Workspace1 1 


(3) 控制 器 S 函数 : chap4 2ctrl. m, 


function [sys,x0, str,ts] = spacemodel(t, x,u, f lag) 


switch flag, 
case 0, 
[sys, x0, str, ts] = ndlInitializeSizes; 
case 1, 
sys = ndlDerivatives(t,x,u); 
case 3, 
sys = ndlOutputs(t,x,u); 
case (2,4,9) 
sys-[]; 
otherwise 
error(['Unhandled flag = ',num2str(flag)]); 
end 


function [sys, x0, str, ts] = mdlInitializeSizes 
sizes - simsizes; 

sizes.NumContStates = 50; 
sizes.NumDiscStates  - 0 
sizes.NumOutputs = 3 
sizes.NumInputs -3; 
sizes.DirFeedthrough = 1 
sizes.NumSampleTimes - 0 
SyS 7 simsizes(sizes); 
x0 = [0.1*0nes(50,1)]; 
str = []; 

ts = [E 


function sys = mdlDerivatives(t, x,u) 
gamal - 10; 


gama2 = 1; 
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ym=u(1); dym= 0.1X cos(t); ddym= - 0.1 X sin(t); 
x17u(2); x2 = u(3); 
e-ym- xl; de= ym- x2; 


kl = 2; 
k2=1; 
k= [k2;k1]; 
E-[e,de]'; 


fori-21:1:25 
thtaf(i,1) = x(i); 
end 
for i=1:1:25 
thtag(i,1) = x(i* 25); 
end 
LEILLLILLILLLLLLELLLLLELLLELELLLLELILE! 
A-[0 -k2; 
1 c Xs 
Q= [10 0;0 10]; 
P= lyap(A,Q); 
E B B E T b E E b T E E T T T D b T L B DTE b EE bbk ET 
FS1 = 0; 
for 11=1:1:5 
gsl = - [(x1 + pi/6- (11- 1) * pi/12)/(pi/24)]/2; 
ul(11) = exp(gsl); 
end 


for 12s 1:1:5 
gs2= - [(x2* pi/6 - (12- 1) * pi/12)/(pi/24)]^2; 
u2(12) = exp(gs2); 

end 


for 11£71:1:5 
for 12= 1:1:5 
FS2(5# (11- 1) * 12) =ul(11) * u2(12); 
FS1 = FS1 + ul(l1) * u2(12); 
end 
end 


FS = FS2/(FS1 + 0.001); 


fx1 = thtaf' * FS'; 
gxi = thtag' * FS' + 0.001; 


ut = 1/gx1 * ( - fx1 + ddym + k' * E); 
b= [071]; 
S1 = — gamal * E'* P * b* FS; 


S2 = -gama2* E' xP* b* FS * ut; 


for i=1:1:25 


60 «j| 机 器 人 控制 系统 的 设计 与 MATLAB 仿 真 ， 基 本 设计 方法 (第 2 版) 


sys(i) = S1(i); 
end 
for j=26:1:50 
sys(j)= S2(j- 25); 
end ] 


function sys = mdlOutputs(t,x,u) 

ym 7 u(1); 

dym 7 0.1* cos(t); ddym= -0.1* sin(t); 
x17u(2); x27 u(3); 

e= ym- x1; de= dym- x2; 


k1=2; 
k2= 1; 
k= [k2;k1]; 
E=[e,de]'; 


for i=1:1:25 
thtaf(i,1)=x(i); 
end 
for i=1:1:25 
thtag(i,1) =x(i+25); 
end 


FS1 = 0; 

for1121:1:5 
gsl = -[(xl* pi/6 — (11 - 1) * pi/12)/(pi/24) ]^2; 
u1(11) = exp(gs1); 

end 


for 1221:1:5 
gs2- - [(x2- pi/6 - (12 - 1) * pi/12)/(pi/24)]^2; 
u2(12) = exp(as2) ; 

end 


forl1121:1:5 
for 12= 1:1:5 
FS2(5* (11-1) * 12) = u1(11) * u2(12); 
FS1 = FS1 *ul(11) * vu2(12); 
end 
end 
FS = FS2/(FS1 + 0.001); 


fx1 = thtaf' * FS'; 
gx1 = thtag' * FS' + 0.001; 


ut = 1/gxl* (— fx1 + ddym + k' * E); 
sys(1) = ut; 


sys(2) = fx1; 
sys(3) = gx1; 
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(4) 被 控 对 象 S 函数 : chap4_2plant. m, 


* S- function for continuous state equation 
function [sys, x0, str, ts] = s function(t, x, u, f lag) 


switch flag, 
% Initialization 
case 0, 
[sys, x0, str, ts] = mdlInitializeSizes; 
case 1, 
sys = mdlDerivatives(t, x,u); 
% Outputs 
case 3, 
sys = mdlOutputs(t, x,u); 
% Unhandled flags 
case {2, 4, 9} 
sys = []; 
% Unexpected flags 
otherwise 
error(['Unhandled flag = ',num2str(flag)]); 
end 


function [ sys, x0, str, ts] = mdlInitializeSizes 


sizes = simsizes; 


sizes. NumContStates 
sizes. NumDiscStates = 


2 
0 
Sizes. NumOutputs = g 
sizes.NumInputs = 1 
sizes.DirFeedthrough = 0 
sizes. NumSampleTimes = 0 
Sys = simsizes( sizes); 

x0= [0.15 0]; 


str-[]; 

ts-[]; 

function sys = mdlDerivatives(t, x,u) 
g79.8; 

m-1; 

120.25; 

d= 2.0; 


I-4/3*m*1^2; 
tol-u(1); 


fx-1/I*(-d*x(2)-m*g*1*cos(x(1))); 
gx= (1+x(1) + x(2))/1; 
sys(1) = x(2); 

sys(2) = fx + gx * (tol- told); 

function sys = mdlOutputs(t, x, u) 

g-79.8; 
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m-1; 

120.25; 

d= 2.0; 
I-4/3*mx*1^2; 


tol-u(1); 


fx-1/I*(-d*x(2) -m*»g*1*cos(x(1))); 
gx 7 (1 * x(1) * x(2))/1; 


sys(1) - x(1); 
sys(2) = x(2) 
Sys(3) = fx; 
sys(4) = gx; 


G) 作 图 程序 : chap4. 2plot, 


close all; 


figure(1); 

subplot(211); 

plot(t,y(:,1), r!,5,y[:,2),.'b!); 
xlabel('time(s)');ylabel('angle tracking'); 


subplot(212); 
piot(t,0.1* cos(t),'r',t,y(:,3), 'b*); 
xlable('time(s)');ylabel('angle speed tracking'); 


figure(2); 
plot(t,tol(:,1),'r'); 
xlabel('time(s)');ylabel('control input'); 


figure(3); 
plot(t,fx(i,1),'r',t,fx(:,2),'"br); 
xlabel('time(s)'); ylabel('fx and estiamted fx'); 


figure(4); 
plot(t,gx(:,1), 'r',t,gx( :, 2), 'b'); 
xlabel('time(s)');ylabel('gx and estimated gx'); 


4.3 单 级 倒立 摆 的 监督 模糊 控制 


倒立 摆 仿 真 或 实物 控制 实验 是 控制 领域 中 用 来 检验 某 种 控制 理论 或 方法 的 典型 方案 。 
倒立 摆 系 统 是 行走 机 器 人 等 许多 控制 对 象 的 最 简单 的 模型 ,是 一 个 复杂 的 非 线 性 的 、 不 确定 
的 高 阶 系统 ,系统 中 的 参数 也 具有 不 确定 性 ,因此 倒立 摆 控 制 器 的 设计 应 保证 有 良好 的 鲁 棒 


4.3.1 模糊 系统 的 设计 


设 二 维 模糊 系统 为 集合 U — [a, ,Bi X [as B; ]CR 上 的 一 个 函数 ,其 解析 式 形式 未 知 。 
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假设 对 任意 一 个 x EU, 则 可 设计 一 个 模糊 系统 。 模 糊 系 统 可 由 以 下 两 步 来 构造 : 
(1) 在 [a; .B,J 上 定义 N, (i 二 1,2) 个 标准 的 ,一 致 的 和 完备 的 模糊 集 ALA A o 
(2) 组 建 M — NX N, 条 模糊 集 IF-THEN 规则 .第 2,2. 条 规则 表示 为 : 
R?” : IF æ, is A} AND az; is Až ,THEN yisB'’ 
其 中 ,i 二 1,2,… ,Nis 二 1,2,…,N, ,将 模糊 集 B'” 的 中 心 (用 y'* 表示 ) 选 择 为 
y'*=g(lel, ej) (4. 49) 
采用 乘积 推理 机 . 单 值 模糊 器 和 中 心平 均 解 模糊 器 ,根据 M =N, XN, 条 规则 构造 模 
糊 系统 f(x),f(zx) 具 体 表 示 如 下 : 


Ni Na 

Dr Duy ^ Gu Gg Gn) 

[x11 £ 

— 75 (4. 50) 
pr, Grips, Gr) 


uuu Cr) 一 


i 
1 


4.3.2 模糊 监督 控制 器 的 设计 


从 概念 上 讲 ,至少 有 两 种 不 同 的 方法 确保 模糊 控制 系统 的 稳定 性 : 第 一 种 方法 是 为 模 
糊 控制 器 选择 特殊 的 结构 和 参数 ,使 带 有 模糊 控制 器 的 闭环 系统 稳定 ; 第 二 种 方法 是 设计 
模糊 控制 器 时 先 不 考虑 稳定 性 ,而 是 将 另 一 个 非 模糊 控制 器 添加 到 模糊 控制 器 上 以 满足 稳 
定性 需要 。 第 二 种 方法 中 模糊 控制 器 的 设计 有 很 大 的 自由 度 和 灵活 性 ,所 以 用 此 方法 设计 
的 模糊 控制 系统 可 获得 更 好 的 性 能 。 
第 二 种 方法 的 关键 是 设计 添加 的 第 二 层 非 模糊 控制 器 ,使 稳定 性 得 到 保证 。 模 糊 控制 
器 执行 主要 控制 操作 ,是 主 控制 器 ,第 二 层 控 制 器 执行 监督 功能 ,如 果 模 糊 控制 器 运行 良好， 
则 第 二 层 控 制 器 停止 工作 ,如 果 模 糊 控制 系统 趋 于 不 稳定 , 则 第 二 层 控制 器 开始 工作 ,以 确 
保 稳定 性 。 第 二 层 控制 器 称 为 监督 控制 器 。 
考虑 如 下 非 线 性 系统 : 
rU —f(rg.eUU)-4 gos...) (4.51) 
其 中 ,x ER 是 系统 输出 ,xu ER HIWA r =lat seer)" ERRERA HE. f M g 
为 未 知 非 线 性 函数 ,并 假设 g 0. 
在 系统 (4.51) 中 .假设 |f(x) | 的 上 界 和 g(x) 的 下 界 已 知 , 即 存在 可 确定 函数 f" (x) 
BI g, GO AE | AGO | € f" G0 0g, GO Sg GO, 
主 模糊 控制 器 设计 为 
U =ü x) (4. 52) 
为 了 确保 闭环 系统 的 稳定 性 ,需要 设计 一 个 控制 器 , 且 要 求 带 有 此 控制 器 的 闭环 系 
统 是 全 局 稳定 的 。 在 模糊 控制 器 wi(x) 上 添加 一 个 监督 控制 器 w(x),u.(x) 只 是 在 状态 
变量 达到 约束 集 {x:|x | 三 M,} 的 边界 时 才 不 为 零 , 其 中 ,M, 为 设计 者 给 定 的 大 于 零 的 
监督 控制 器 设计 为 
uU 十 了 ux) (4. 53) 
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其 中 , 厂 为 指示 函数 。 

控制 的 主要 任务 仍然 由 模糊 控制 器 wi,(x) 承 担 ,通过 设计 监督 控制 器 u. ,使 对 所 有 的 
t 之 0, 有 x 三 M,。 监 督 控制 器 式 (4. 53) 的 控制 策略 为 

(1) X | x | >M, 时 ,1*=1。 l 

(2) 4 || x || <M, 时 ,太一 0。 

将 式 (4.53) 代 入 式 (4. 51) ,得 

x" — f H gx) ur D - 1" u(x)) (4. 54) 
为 了 证 明 稳 定性 ,需要 将 闭环 系统 的 方程 写成 向 量 形式 。 
由 被 控 对 象 式 (4. 51) 的 表达 形式 ,可 定义 使 闭环 系统 稳定 的 理想 控制 器 为 


= (— f(x)—k"x) (4.55) 
gx) 


其 中 ,一 (RD)TER" ,使 得 多 项 式 ;十 kis” UU dE, 的 所 有 根 都 在 复 平面 的 左 
半 平 面 上 。 
将 式 (4.55) 代 入 式 (4. 540 ,得 
X ”一 xz) 十 gCz)xz —g(x)u* t gGO[uus GO dI" ux)] 


——k'x-FgGO[uu OO —u"' +I*u,] (4. 56) 
定义 
[0 1 0 0 0 0] 
0 0 1 0 0 : 
A=| : : TE L ££]. g=. (4, 57) 
0 0 0 0 = 0 1 x 
T S SN —&] rx) 
则 式 (4. 56) 可 以 写成 向 量 形式 
X —AX c b[uuu(x)—u' c I'u,] (4. 58) 


4.3.3 稳定 性 分 析 
定义 Lyapunov 函数 为 


Vx Pr (4. 59) 
其 中 ,P 是 满足 Lyapunov 方程 的 正定 对 称 和 矩阵 , 且 满 足 


其 中 ,@ 二 0 由 设计 者 给 定 。 因 为 A 是 稳定 的 ,所 以 这 样 的 P 总 是 存在 的 。 
利用 式 (4. 58) 和 式 (4. 60) ,并 考虑 到 |‖x | >M, 时 ,1* —1.48 


p od sn 3 mas 
V= 7* Px- 2* Px 
1 . T T a " 
—3 Ax t bGru, —u cu) Px yx P(AxX+ blús —u" Tu, 


i xl lox 
—3* (A'P-PA )x HSO Pru —u Tu) cx Ph(us—u. +u.) 
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=— Fx"Qx + xP bu — u^ +u,) 


x bl o - 
VxE—.x'Qxt Ix'Pb|CGugu|-- |u' ^tx'Pbu, 


为 保证 V0, 设计 监督 控制 项 u, 为 


u, x) == sga P b (Tf" + KE Do [tural ) (4.61) 


L 


则 


V«- yx Qe [T Pb uus He |^ D— Pb (p De La) 


< yx 0x «o (4. 62) 
3X (4. 53) PH 17 dé — 1 BE EEK PR. E x EAA || x | — M, 时 ,监督 控制 器 就 开始 


工作 ,每 当 x 回 到 约束 条 件 | x |‖ =M, 的 内 部 时 ,监督 控制 器 就 停止 工作 ,因此 系统 在 跨越 
边界 时 将 受到 冲击 。 克 服 这 种 “振荡 ”的 一 个 办 法 就 是 , 令 三 在 0 一 1 连续 变化 。 可 以 选择 


如 下 : 
| lx || <a 
pui 2 a « |x] <M, (4. 63) 


M 
! | x ll >M, 
其 中 ,a EOM ) 为 设计 者 给 定 的 一 个 常数 。 厂 形 如 式 (4.51), 则 当 x 从 a ZESIM, 时 , 监 
督 控制 器 u. 将 从 停止 状态 连续 变化 到 “最 大 值 ?1 。 
通过 上 述 分 析 可 知 ,通过 设计 监督 控制 器 ,可 以 保证 | x || S M,. 
根据 LaSalle 不 变性 原理 ,由 式 (4. 62) 可 知 , 当 V 三 0 Bb x 0, M] >h}, x0, AE 
的 收敛 速度 取决 于 @ 。 


4.3.4 仿真 实例 


被 控 对 象 取 单 级 倒立 摆 , 如 图 4-11 Bros ,其 控制 目标 为 使 摆 直 立 ,并 保证 静止 。 单 级 倒 
立 摆动 态 方程 为 


XTi ST? 
2 . 
z mix5cosr,sinz,; COSI, 
gSsinz, 
, m. +m m. +m 
d =Œ 2 2 u 
( 4 mcos x, ) ( 4 mcos Xl ) 
3 m. +m 3 m. +m 


其 中 ,z， 和 > 分别 为 摆 角 和 摆 速 ,二 9. 8,m. 二 1 为 小 车 质量 , m 为 摆 杆 质量 ,六 一 0. 1,1 
为 摆 长 的 31 — L0. 5,8 为 控制 输入 。 
模糊 控制 ui 是 依据 以 下 4 条 模糊 规则 设计 的 : 
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图 4-11 单 级 倒立 摆 系 统 示意 图 


(1) 如 果 c, 是 正 且 c. 是正 , 则 w 是 负 最 大 值 。 
(2) WR r, 是 正 且 r: EmN u EF, 
(3) 如 果 r, 是 负 且 r: 是正, 则 是 零 。 
(4) WR zi 是 负 且 r: 是 负 , 则 w 是 正 最 大 值 。 


模糊 集 * 正 负 ”"“ 负 最 大 值 ”“ 零 "和 * 正 最 大 值 "的 隶属 函数 分 别 为 六 ，(z) = 


Eee agua (=e ^, 


(u t5) 


l 
Ha GO — p oue IH nag 00 5e 


要 设计 监督 控制 器 ,首先 要 确定 边界 /” 和 g,。 对 本 系统 ,如 果 要 求 |x, | 三 x/9, 则 有 


dis mlx, cosx sint, d 二 0.12«0.5 2X 0, 5sin(C22,) a 
Tomp = à 一 一 天 
e+ 0. R 
IX Gi sz I me = E b > 
| EE ($an Sen) 
3 m. +m - 3 14-0.1 
0.025 , 
9.84 L] d 
2 0.05 
3 Ll 
— 15, 78 +0. 03662 = f" (xr, x) 
COST, cos(x/9) 
leti, ses m, Fm x 0. T4E4 
0, T2 2 
4 mcos r, 4 0.1 ;, 
i(- ) 0, 5 X -rls 
3 m. +m E 1.1 ) 
s(x/9) 
= cost, z =]. 1 =g, Crziyzy) 
1,4. 2€ (s+ Li cos x) 


控制 的 目标 是 能 将 任意 初始 角度 zi € [一 xy/9,x/9] 的 倒立 摆 控 制 到 平衡 点 , 取 z(0) 一 0， 
则 || (zzz) l|; x/9. W M, —m/9. 

采用 乘积 推理 机 , 单 值 模糊 器 和 中 心平 均 解 模 糊 絮 ,根据 M=N, XN, (N, =2, N; =2) 3% 
规则 来 构造 模糊 控制 器 wi,,。 由 四 条 规则 的 结论 可 知 : y ——5.y ^ —0.y" 二 0,y” 一 5， 
则 由 式 (4. 50) 得 模糊 控制 器 为 


y" ng Ca ng Cp Gr2) ty pr rae) yn Grip Cr) 
Ug, 一 


Bg ted c pa Grip Gri) FE pg (Ta ET pn (Ti (Es) 


第 4 章 机械 手 模糊 自 适 应 控制 e 67 


—9Spu y Gridgpg Go? F 5p aq (Lid (ey 
Pal True tr) gy Grip C3) d- py Grip Gr) H- pq Tr pn Gr?) 
1 1 1 1 


5 = —3 l » 
z 1 ED e 30.r 1 Es e 30: 1 JL: eer 1 二 e 工 
1 l 1 1 1 1 1 l 
30 —30.r 30 —30.r + —307 30r T 30.r 30.r 
1 十 e 1] 十 e l--e*1se 1 十 e 1] 十 e lte” 1-e 
Rs di st JS eR CE H FEJT 04 90] sj Ji PR R K . n p 4-12 和 图 4-13 所 示 。 倒 立 摆 初 始 状 


10 0 


态 为 Lx/60.0], 采 用 控制 律 式 (4. 500 ,3X (4.532 3X (4. 61) 。 取 Q= | | si = — 1. Di] 


0 10 


' TE: 
求解 Lyapunov 方程 式 (4.60) 得 : P= f ; o ZIE xri (0) —2x/60, n Bt x, (0) 0, M, =7/9, 


9 9 
4 一 f/18 ,仿真 结果 如 图 4-14 一 图 4-16 所 示 。 
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membership function degree of x 
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图 4-12 角度 x, 的 隶属 函数 


membership funciton degree of u 


0 
u 


图 4-13 控制 输入 u 的 隶属 函数 


UA 


angle response 
o 
© 
un 


© 


20 . 30 
time(s) 
图 4-14 角度 响应 
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0.15 


0.1 


angle speed response 


0 10 20 30 40 50 
time(s) 


B] 4-15 角速度 响应 


control input 


0 10 20 30 40 50 
time(s) 


图 4-16 控制 输入 信号 


仿真 程序 如 下 : 
CD 隶属 函数 设计 程序 : chap4_3mf. m, 


clear all; 
close all; 
T-0.001; 


x= — pi/9:T:pi/9; 
figure(1); 
fori-1:1:2 


if i== 

niux= 1./(1-* exp( - 30 * x)); 
elseif i== 

niux - 1. /(1 + exp(30 * x)); 
end 
hold on; 


plot(x,niux); 
end 


xlabel('x');ylabel('membership function degree of x'); 


us -55*51:5; 
figure(2); 
for i=1:1:3 
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re 


if i== 
niuu= exp( - (u* 5).^2); 
elseif i== 
niuu- exp( - u. ^2); 
elseif i-- 
niuu- exp( = (u- 5).^2); 
end 
hold on; 
plot(u, niuu); 
end 
xlabel('u'); ylabel( membership function degree of u'); 


(2) Simulink 主 程序 : chap4. 3sim. mdl, 


To Workspace& 


chap4 3plant 


S-Functiont 


To Workspace? 


chap4 3ctrl 


8-Function 


Clock To Workspace 


(3) 控制 器 SAŠU: chap4 3ctrl. m, 


function [sys, x0,str,ts] = spacemodel(t, x,u, flag) 
switch flag, 
case 0, 
[sys, x0, str, ts] = ndlInitializeSizes; 
case 3, 
sys = ndlOutputs(t,x,u); 
case (2,4,9) 
sys- []; 
otherwise 
error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0, str, ts] = ndlInitializeSizes 


sizes - simsizes; 


sizes.NumContStates 


sizes.NumDiscStates 
sizes.NumOutputs 


外 


LU 
Oor NUÀOO 
EM 


sizes.NumInputs 


sizes.DirFeedthrough 


sizes. NumSampleTimes 
SyS 7 simsizes(sizes); 
x0 - []; 
str = [i 
ts = [li 
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function sys = mdlOutputs(t, x, u) 
x1l-7u(1); 
x2 = u(2); 


kl = 2;k2= 1; 

k= [k2;k1]; 

Ts 2 T B 2 D T T b B B D T bk Tb Tbb bb DT bbb hD Tb Dyk 
ul 1-21/(1*exp(-30*x1)); 

ul 2-7 1/(1 + exp(30 * x1)); 


u2 1-71/(1-* exp( - 30 * x2)); 
u2 2-2 1/(1* exp(30 * x2)); 


Fnum = -5*ull1*u21*5*ul2*u2 2; 
Fden-ul 1*u2 1*ul12*u21*ull*u22-*u12*u2 2; 
ufuzz - Fnum/Fden; 
LELIILILELLLLILLILLLLELLLLLLLLLLELLEI 
R= [0 -k2; 
l —11]; 
Q- [10 0;0 10]; 
P= lyap(A, Q); 


b= [0;1]; 
x-7[xl;x2]; 


gL-71.1; 
fU- 15.78 * 0.0366 * x2^2; 
us- - sign(x' * P * b) * (1/gL * (fU + abs(k' * x)) * abs(ufuzz)); 


a= pi/18; 
Mx = pi/9; 
S-1; 
if S-- 
if norm(x)» = Mx 
Iz-21; 
else 
I=0; 
end 
elseif S== 
if norm(x)<a 
I=0; 


elseif norm(x)« Mx&norm(x)>=a 
I= (norm(x) - a)/(Mx- a); 
else 
Dus 
end 
end 
ut = ufuzz + I * us; 
sys(1) = ufuzz; 


sys(2) = us; 
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sys(3) = ut; 
(4) 被 控 对 象 S PRX: chap4. 3plant. m, 


function [sys,xO,str,ts] = s function(t, x, u, f lag) 
switch flag, 
case 0, 
[sys, x0, str, ts] = ndlInitializeSizes; 
case 1, 
sys = ndlDerivatives(t, x,u); 
case 3, 
sys = ndlOutputs(t,x,u); 
case (2, 4, 9 ) 


sys = []; 
otherwise 

error(['Unhandled flag = ',num2str(flag)]); 
end 


function [sys, x0, str, ts] = mdlInitializeSizes 
Sizes - simsizes; 


O OW N O t 
S. 


sizes.NumContStates 
sizes.NumDiscStates  - 
sizes. NumOutputs = 
sizes. NumInputs = 
sizes.DirFeedthrough = 
sizes. NumSampleTimes = 
sys = simsizes( sizes); 
x0 = [pi/60 0]; 

str-[]; 

ts=[]; 

function sys = mdlDerivatives(t, x,u) 
g-79.8; 

mc = 1.0; 

m-0.1; 

120.5; 


S-1* (4/3-m* (cos(x(1)))^2/(mc + m)); 
fx72g*sin(x(1))-m*1»*»x(2)^2 * cos(x(1)) * sin(x(1))/(mc +m); 


fx- fx/S; 
gx 7 cos(x(1))/(mc +m); 
gx = gx/S; 
ut - u(3); 


sys(1) = x(2); 

sys(2) = fx t gx * ut; 

function sys = mdlOutputs(t, x, u) 
sys(1) * x(1); 

sys(2) = x(2); 


C5) 作 图 程序 : chap4 3plot. m, 


close all; 
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figure(1); 

plot(t, y( :,1), 'z'); 
xlabel('time(s)');ylabel('angle response'); 
figure(2); 

plot(t,y(:,1),'r"); 
xlabel('time(s)');ylabel('angle speed response'); 
figure(3); 

subplot(311); 

plok(t, u( :41),.'r"); 
xlabel('time(s)');ylabel('control input ufuzz'); 
subplot(312); 

plot(t,u(:,2), 'r'); 
xlabel('time(s)');ylabel('control input us'); 
subplot(313); 

plot(t,u( :,3),,'r^); 
xlabel('time(s)');ylabel('control input ut'); 


4.4 基于 模糊 补偿 的 机 械 手 自 适 应 模糊 控制 


通过 对 文献 [24] 的 部 分 控制 方法 进行 详细 推导 及 仿真 分 析 ,研究 基于 模糊 补偿 的 机 械 手 
控制 的 设计 方法 。 由 于 传统 的 模糊 自 适 应 控制 方法 对 于 存在 较 大 扰动 等 外 界 因素 时 ,控制 
效果 较 差 。 为 了 减弱 这 些 外 界 干扰 因素 的 影响 ,可 以 采用 模糊 补偿 器 ,同时 为 了 减少 模糊 到 
近 的 计算 量 , 提 高 运算 效率 ,采用 对 不 同 的 扰动 补偿 项 加 以 区 分 .分 别 逼 近 的 方法 。 仿 真 试 
验 表明 这 种 改进 的 带 模糊 补偿 器 的 自 适应 模糊 控制 方法 可 以 很 好 地 抑制 摩擦 、 扰 动 .负载 变 
化 等 因素 影响 。 


4.4.1 系统 描述 


机 械 手 的 动态 方程 为 
D(q0q —- C(q.q)d +G) -- F(q.d.q) —c (4. 64) 
其 中 ,D(d ) 为 惯性 力矩 ,CCq,d ) 是 回 心 力 和 哥 氏 力矩 ,GCq0 J& 3E 7], F (qq ,9) 是 由 摩擦 
F, .扰动 tr, 、 负 载 变 化 的 不 确定 项 组 成 ,q 为 关节 角度 。 
4.4.2 基于 传统 模糊 补偿 的 控制 


假设 D(g)、C(gq,g) 和 G(gq) 已 知 , 且 所 有 状态 变量 可 测 得 。 


定义 滑 模 函数 为 
s—4q--Aq (4. 65) 
其 中 ,A 为 正定 阵 ,qg (4) 为 跟踪 误差 ,gqg 二 g 一 ga,q4 为 理想 角度 。 
定义 
daD =D Aa) (4. 66) 


定义 Lyapunov PR 


va =3 (sDs + X Or 8.) (4. 67) 
i-1 
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K'p.8,—8; 一 9,,@ ”为 理想 参数 ,了 ,之 0。 
HIT s—q--Aq-—d4-—4.-Aq-4 一 4;;, 则 
S 一 和 十 AI 一 和 一 和 十 AI 一 和 一 4， 
Ds —Dd — Dg, =t — Cå —G —F —D4, 


于 是 
VG) 一 sTDS 十 sDs + 3 6r, Â, 
i=l 


——s'[— e 4- C - G 4- F(q,4 4) 4- Dj, — Cs] 4- 9, ÖT, 6, 
i=l 


— —s"[Dà, 4- Cd, +G +F) —r]+ > 8T, 6, (4. 68) 
i=l 


其 中 ,F (gq,G ,9) 为 未 知 非 线 性 函数 ,采用 基于 MIMO 的 模糊 系统 下 (q,4 d 18 ) 来 通 近 
F(q.q.q). 
下 面 设 计 两 种 基于 模糊 补偿 的 自 适 应 控制 律 。 
1. 自 适应 控制 律 的 设计 
设计 控制 律 为 
c —D(D4, - C(q.d)d, +G) HF(g,G,9 | 8) — Kos (4. 69) 
H'B.K,5—diag( K,)0,K,20,i—1.2,*,n., H 


Fi(q.d.q 10) 8! € (q.d.q) 


Led 3 ,0 0 p ( sat) 
ARTI E ai T a i (4. 70) 
Ê, Cg ICI | 8,) | 8; € (q.d.d) 
模糊 逼近 误差 为 
w=F(g,G,9) —F(Gq.d.d | 87) (4. 71) 


将 控制 律 式 (4. 690 AR C4. 680 ,得 
VG) 一 一 sSTCGF(q d) -Fiil 80 -Kys) + > 8T, Ô, 
i21 
——s'G Gd —FG.41620 FG.4.410870— 


F(q.d.d 10°) -Ko$) + 9] 8T, 6, 
1 一 1 


— —s' (8'£ (q.d. d) +w +Kps) + > OT, Ô, 


i=1 
一 一 SIKus 一 SIw 十 kx (8r, 8. — 5,07€ (q.d.q)) 
i=l 


其 中 ,@=@ "一 9 ,6 (q.d ,9) 为 模糊 系统 。 
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自 适 应 律 为 
Ô; ——D;s(G.d.d), i=1,2, ,Nn (4. 72) 
则 
V()—-—s'Kyps—s'w 
只 要 将 Ko EH Jg EK , TEIE VO, colt, s0, ATi q-0.q-70, AA ic efc RE HC 
决 于 Ks。 由 于 V 宇 0.V 过 0, 则 V 有 界 , 因 此 s 和 8, 有 界 ,但 无 法 保证 8, KATE. 
2. 鲁 棒 自 适应 控制 律 
为 了 消除 通 近 误差 造成 的 影响 ,保证 系统 稳定 ,在 控制 律 中 采用 了 和 鲁 棒 项 。 设 计 鲁 棒 自 
rz 一 D(q)9, -C(Q.d)d, +G) Fa | 80 — Kos —Wsgn(s) (4.73) 
其 中 W — diag| ww, PM, ew, d] zo, |5i—1,2,*.n., 
将 控制 律 式 (4.73) 代 入 式 (4. 68) ,得 
Va)=—s"Kps <0 

收敛 性 分 析 同 上 面 第 1 部 分 。 

分 析 : 假设 机 械 手 关节 个 数 为 六 个 ,如 果 采 用 基于 MIMO 的 模糊 系统 下 (q,4 418) 
KEE FG.q ,9), 则 对 每 个 关节 来 说 ,输入 变量 个 数 为 3。 如果 针对 ?7 个 关节 机 械 手 ,对 
每 个 输入 变量 设计 &A 个 隶属 函数 , 则 规则 总 数 为 &” 。 

例如 ,机械手 关节 个 数 为 2, 每 个 关节 输入 变量 个 数 为 3, 每 个 输入 变量 设计 5 T 3R ES PR 
数 , 则 规则 总 数 为 5” 二 5 二 15625, 如 此 多 的 模糊 规则 会 导致 计算 量 过 大 。 为 了 减少 模糊 
规则 的 个 数 , 应 针对 (gq ,gq ,9,t) 的 具体 表达 形式 分 别 进行 设计 。 


4.4.3 基于 模型 信息 已 知 的 模糊 补偿 控制 


假设 D(g)、C(q ,gq ) 和 G(g) 为 已 知 , 且 所 有 状态 变量 可 测 得 。 

1. 基于 摩擦 的 模糊 补偿 控制 

只 考虑 针对 摩擦 进行 模糊 远近 的 模糊 补偿 控制 ,由 于 摩擦 力 只 与 速度 信号 有 关 , 则 用 于 
逼近 摩擦 的 模糊 系统 可 表示 为 五 (4 100 ,可 根据 基于 传统 模糊 补偿 的 控制 器 设计 方法 , 即 用 
式 (4. 69) . 式 (4.72) 和 式 (4.73) 设 计 控 制 律 。 

模糊 自 适 应 控制 律 设计 为 


r 一 D(g)g, - C(.d)4, +G) +F | 0) — Ks (4. 74) 
鲁 棒 模 糊 自 适应 控制 律 设 计 为 
c —D(G04, +C). +G) HF Cj | 0) — Kos — Wsgn(s) (4. 75) 
自 适应 律 设 计 为 


0, ——T';'si£(d), i-—1.2,-,n (4. 76) 
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模糊 系统 设计 为 
TNI 8! Eca) 


s ACT 0; È (q) 
F(q105— PF. 5 ne Me 


Pd) edo 
2. 基于 外 加 干扰 的 模糊 补偿 控制 
只 考虑 针对 外 加 干扰 进行 模糊 逼近 的 模糊 补偿 控制 ,如 果 外 加 干扰 与 角度 和 角速度 信 
号 有 关 , 则 用 于 逼近 外 加 干扰 的 模糊 系统 可 表示 为 (gq ,6 1 ) ,可 根据 基于 传统 模糊 补偿 
的 控制 器 设计 方法 , 即 用 式 (4.69) ,3X (4. 72) RIS C4. 730 EE Pl 
模糊 自 适应 控制 律 设计 为 
r=D(g)g, - C(q.d)4d, - G() HF (q.d | 8) — Kys (4. 77) 
鲁 棒 模 糊 自 适应 控制 律 设 计 为 
r=D(g)g, - C(Q.d)4, +G) HF Q.4 | 89) — Kys — Wsgn(s) (4. 78) 
自 适 应 律 设计 为 


6, ——D;'s£(q.d). i=1,2,,n (4. 79) 
模糊 系统 设计 为 
F,(q.q 19,) 8/£(q.q) 
7T F.(Q.18. 8: € (q.d) 
gd [= [^s 1897]. Sud 


gal6)| 18576 G4) 
3. 基于 摩擦 .外 加 干扰 和 负载 变化 的 模糊 补偿 控制 
考虑 机 械 手 不 确定 部 分 同时 包括 摩擦 、 外 加 干扰 和 负载 变化 的 情况 ,由 于 负载 变化 与 加 
速度 有 关 , 则 用 于 逼近 外 加 干扰 的 模糊 系统 可 表示 为 下 Co,d dl O ), 为 了 减少 模糊 规则 的 
数量 ,将 不 确定 项 F(q ,gq .q10 ) 进 行 分 解 ,并 根据 基于 传统 模糊 补偿 的 控制 器 设计 方法 , 即 
HRA. 69) . 式 (4.72) 和 式 (4.73) 设 计 控 制 律 。 
机 械 手 动态 方程 为 
D(q0q - C(q.q)0q +G) -- e(q.d.qd-t) + F,(q) - c, —c (4. 80) 
其 中 ， 
D(q) —Dnm,.q) 
C(q.q) —CGO,.q.d) 
G(q) —Gn,.q) 


e(q.d.q.0) —ep[ DIqoq ] -- ec[CC(q.d)04 ] - es[ GCq) ] 
而 且 
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ep —DGn,..q)q — Dn, .q)0q 
ec —CGn, q.d0d — CCm,.q.q)0q 
es — Gn, q) — GGn,.q) 
e(q.d.q) =e [Dq] -- ec[CC(q.d)d ] 3- es[GCq) ] 
m, 为 已 知 的 名 义 值 ,ms 为 实际 值 。 
不 确定 部 分 可 表示 为 
F(q qq) —e(q T sqt) 十 F,(g) d Ta 
上 式 可 分 解 为 
F(q.d.q) —F'(q.q) +F" (q.q) 
其 中 ， 
F'(q.q) —ec[C(q.q)04 ] -- e.[GCqD) ] -- F,(q) 十 rs 
F*(q.q) —ey[ DC(kq0q] 
模糊 自 适 应 控制 律 设 计 为 
c—DGD4, +C). - GQGD +F (gd | 80 +F (q.d | 8) — Kos 
自 适 应 律 设 计 为 
6| ——r;s£&(q.. i—1,2,-.n 


8! =T zs E q), i-l2,.-n 
定义 Lyapunov 函数 为 


V(t) =- 


zs ns 十 Si "T, 8: + E} oTa 8:) 


i=l i=} 
则 


VG) ——s'(Gd£, 十 Ch, 十 G 十 下 一 rz ) 十 p ƏT, 8: + S 8T., 8: 
模糊 逼近 误差 分 别 为 B B 
w! - F'(q.d) —P (q.d | 8) 
w —F(qd)—F (q.d 18^) 
则 


VG) ——s'Kys —s! (w! +w) + X, (80T, Ô! —5,8" 8 (q.d) 十 


i=l 


N OT, 8: — s, OTE q) 


i=l 


——s'Kys —s' (w! +w) 


(4. 81) 


(4. 82) 


(4. 83) 


(4. 84) 
(4. 85) 


如 果 设 计 足 够 大 的 Kb, 则 roots 0. A ifi q70.q--0.. RA RO ic oic REG 
Kb。 由 于 V 宇 0,V 过 0, 则 VV 有 界 , 因 此 87 和 67 有 界 , 但 无 法 保证 87 M O 收敛 


于 零 。 
为 了 消除 逼近 误差 造成 的 影响 ,设计 和 鲁 棒 自 适应 律 为 
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c —D(GD4, - C(Q.d)4, +G) FE! qå |@') +F qå | 8) — Kos — WsgnG) 
(4. 86) 
其 中 ,W= diag[ ew, "Wy, pwy Lowy, > |w: |+ |w? | 4i 1,2, m. 
模糊 系统 设计 为 


Fi (q.d 18) - F1 (q.4182) 


Ro a F: (qå |8))-- F2(q,4182) 
FQ.4.41850— Pig ies KEIC 


F: q.d 101) +F? (Q.41802)| 
关于 基于 模型 信息 未 知 的 模糊 补偿 控制 的 设计 及 其 推导 过 程 详 见 文献 [2] ,其 仿真 设计 
可 参考 以 下 的 仿真 实例 。 


4.4.4 仿真 实例 
针对 双关 节 刚 性 机 械 手 ,其 动力 学 方程 为 (4. 64) ,具体 表达 如 下 : 


os 2 M A fous lr d ge TWE + re qp E i 
Dalga) DaG] |g, Ceti, 0 n Fog m 
其 中 ， 
Dj(q;) 2 Gn, 4- mri -- m;ri + 2m;rir;cos(q;) 
Dj; (q;) =Da (q;) 9 m;ri d mirir;cos(q;) 
D (q,)-—m;ri 
Cj; (q5) — m;r,r;sin(q;) 
4 y—laisai] ,rz 一 [rr] ,x 一 [qi,91,42,42] 。 取 系统 参数 为 ri 二 1m,r, 一 0. 8m, 
mi=lkg,m,=1. 5kg. 
控制 目标 是 使 双关 节 的 输出 g,、gs 分 别 跟 踪 期 望 轨迹 qu; —0. 3sint 和 gw 二 0. 3sint 。 
定义 隶属 函数 为 
-i2 
ny GO =exp(— Come) 
其 中 ,zx; 分 别 为 一 x/6, — x/12,0, x/12, x/63i — 1,2,3,4, 54 A, 分 别 为 NB. NS, ZO, 
PS,PB, 
仿真 实例 1. 针对 带 有 摩擦 的 情况 ,采用 基于 摩擦 模糊 补偿 的 机 械 手 控制 , 取 控 制 器 设 
计 参 数 为 X41 二 10,4; 二 10,Kb 二 201 ,T= 二 ,二 0.0001。 取 系统 初始 状态 为 gq,(0) 二 gq,(0)= 


10q 1 十 3sgn(g1) 


41(0) 二 9 ,(0) 二 0, 取 摩擦 项 F(g )== | | 。 在 鲁 棒 控 制 律 中 , 取 W— diag[ 2.2 ]. 


104, --3sgn(q ,) 
模糊 系统 权 值 中 每 个 元 素 初 值 取 0. 1. 

仿真 中 考虑 带 鲁 棒 项 和 不 带 鲁 棒 项 两 种 情况 ,分 别 取 M=1 和 M=2, 采 用 控制 律 
式 (4.74) 、 鲁 棒 控制 律 式 (4.75) 及 自 适 应 律 式 (4.76)。 取 M 王 1, 仿 真 结果 见 图 4-17 一 图 4-19。 
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(b) 双关 节 角 速度 跟踪 
图 4-17 双关 节 角 度 与 角速度 跟踪 
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time(s) 


40 s m Do -————— 
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图 4-18 双关 节 摩 擦 及 其 补偿 
te 100 r ———— 
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B] 4-19. 双关 节 控 制 输入 


基于 摩擦 模糊 补偿 的 机 械 手 控制 的 仿真 程序 如 下 : 
(1) Simulink 主 程序 : chap4_4sim. mdl。 


chap4 4ctd 
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(2) 控制 器 S 函数 : chap4_4ctrl. m, 


function [sys,x0,str,ts] = MIMO Tong s(t,x,u, flag) 
switch flag, 
case 0, E 
[sys, x0, str, ts] = ndlInitializeSizes; 
case 1, 
sys = ndlDerivatives(t,x,u); 
case 3, 
sys = mdlOutputs(t,x,u); 
case (2,4,9) 
sys- []; 
otherwise 
error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0, str, ts] = mdlInitializeSizes 
global nmn1 nmn2 Fai 
nmnl = 10;nmn2 = 10; 
Fai- [nmnl 0;0 nmn2]; 
Sizes - simsizes; 


sizes.NumContStates 


LU 
m 
o 


sizes.NumDiscStates 


sizes.NumOutputs 
sizes.NumInputs 


LU 
oO r 0 5» o 
~ 


sizes.DirFeedthrough 


sizes.NumSampleTimes 


SyS 7 simsizes(sizes); 


x0 = [0.1*0nes(10,1)]; 
ste = [1$ 
ts = [E 


function sys = mdlDerivatives(t,x,u) 
global nmn1 nmn2 Fai 

qdl = u(1); 

qd2 = u(2); 

dqd1 = 0.3 * cos(t); 

dqd2 = 0.3 * cos(t) ; 

dad = [dqd1 dqd2]'; 


ddqdl = - 0.3 * sin(t); 
ddqd2 = - 0.3 * sin(t); 
ddqd = [ddqdl ddqd2]'; 


ql = u(3);dal = u(4); 
q2 = u(5);dq2 = u(6); 
B B B B B b T b B T B b T T T B S b T T T T E T T T S 6 T T o To T o E To h T T b T 6 E 
fsdl = 0; 
for 1121:1:5 
gs1 = — [ (dq1 + pi/6 - (11 — 1) * pi/12)/(pi/24)]^2; 
ul(11) = exp(gsl1); 
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end 
fsd2 = 0; 
for 122571:1:5 
gs2= -[(dq2 + pi/6 - (12 - 1) * pi/12)/(pi/24)]^2; 
u2(12) = exp(gs2) ; 
end 
for 1121:1:5 
fsul(11) » u1(11); 
fsdi = fsdl + u1(11); 
end 
for Iz= Llsg 
fsu2(12) = u2(12); 
fsd2 = fsd2 + u2(12); 
end 
fsl = fsul/(fsd1 + 0.001); 
fs2 = fsu2/(fsd2 + 0.001); 
ELLLLILLLIELLLELLLDLELLLLELLELLELELLELILLLLEELEL 
el =q1- qdl; 
e2 = q2 - qd2; 
e= [el e2]'; 
del = dq1 - dqd1; 
de2 = dq2 - dqd2; 
de = [del de2]'; 


s-detFai*e; 
Gamal = 0.0001;Gama2 = 0.0001; 


S1 = - 1/Gama1 * s(1) * fs1; 
S2- - 1/Gama2 * s(2) * fs2; 
fori-1:1:5 
sys(i)-7S1(i); 
end 
for j=6:1:10 
sys(j)= S2(3- 5); 
end 


function sys = ndlOutputs(t, x,u) 
global nmnl nmn2 Fai 
ql = u(3);dql = u(4); 
q2 = u(5);da2 = u(6); 


rl-1;r2-0.8; 

ml = 1;m2 = 1.5; 

D11 = (ml + m2) * r1^2 + m2 * r2^2 +2 * m2 * rl * r2 *cos(qą2); 
D22 = m2 * r2^2; 

D21 = m2 * r2^2 + m2 * r1 * r2 * cos(q2); 

D12 = D21; 
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D= [D11 D12;D21 D22]; 


C12 = m2 * r1 * sin(q2); 

C= [ -C12 * dq2 - C12 * (dql * dg2);C12 * ql 0]; 

gl = (m1 + m2) * r1 * cos(q2) + n2 * r2 * cos(ql + q2); 
g2 7 m2 * r2 * cos(ql + q2); 

G= [gl;g2]; 


adi = u(1); 
qd2 = u(2); 
dqdl = 0.3 * cos(t); 
dqd2 = 0.3 * cos(t) ; 
dad = [dqd1 dqd2]'; 


ddqdl = - 0.3 * sin(t); 
ddqd2 = - 0.3 * sin(t); 
ddqd = [ddqdl ddqd2]'; 


el-2ql- qdl; 
e2 = q2 - qd2; 
e= [e1 e2]'; 
del = dq1 - dqd1; 
de2 = dq2 - dqd2; 
de = [del de2]'; 


s-detFai*e; 


dqr = dqd - Fai*e; 
ddqr = ddqd - Fai * de; 


fori-1:1:5 
thtal(i,1)-x(i); 
end 
for i-21:1:5 
thta2(i,1)-7 x(i*5); 
end 
fsdl= 0; 


fori1151:1:5 
gsl = - [ (dq1 + pi/6 - (11- 1) * pi/12)/(pi/24)]^2; 
u1(11) = exp(gs1); 

end 

fsd2 = 0; 

for 12 = 171:5 
gs2 = - [ (dq2 + pi/6 - (12 — 1) * pi/12)/(pi/24)]^2; 
u2(12) = exp(gs2); 

end 
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for 11=1:1:5 
fsul(l1) = u1(11); 
fsdl = fsdl + u1(11); 
end 
for 1221:1:5 
fsu2(12) = u2(12); 
fsd2 = fsd2 + u2(12); 
end 
fsl = fsul/(fsd1 + 0.001); 
fs2 = fsu2/(fsd2 * 0.001); 


Fp(1) = thtal' * fs1'; 
Fp(2) = thta2' * fs2'; 


KD = 20 * eye(2); 


W= [2 0;0 2]; 
M=1; 
ifM--1 


tol-D* ddqr*C*dgr*tG-*1*Fp'-KD*s; 
elseif M 
tol = D * ddqr*C*dqrtG*t1*Fp'-KD*s-W*sign( 


end 


sys(1) = tol(1); 
sys(2) = tol(2); 
sys(3) = Fp(1); 
sys(4) = Fp(2); 


(3) 被 控 对 象 S PR. chap4 4plant. m, 


function [sys, x0, str, ts] = MIMO Tong plant(t, x, u, f lag) 
switch flag, 
case 0, 
[sys,x0, str, ts] = ndlInitializeSizes; 
case 1, 
sys = ndlDerivatives(t,x,u); 
case 3, 
sys = mdlOutputs(t,x,u); 
case (2, 4, 9 ) 
sys = []; 
otherwise 
error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys,x0, str, ts] = mdlInitializeSizes 


sizes simsizes; 


sizes. NumContStates 
sizes. NumDiscStates 
sizes.NumOutputs 
sizes.NumInputs 
sizes.DirFeedthrough 


oOo A o0 Lx 


5 (4.74) 


s); % (4.75) 
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sizes.NumSampleTimes - 0; 

Sys = simsizes(sizes); 
x0-[0000]; 

str-[]; 

ts-[] 

function sys = mdlDerivatives(t,x,u) 
rl-1;r2720.8; 

ml = 1;m2 = 1.5; 


D11 = (ml + m2) * r1^2 + m2 * r2^2 +2 *m2* rl *r2*cos(x(3)); 
D22 = m2 * r2^2; 

D21 = m2 * r2^2 + m2 * r1 * r2 * cos(x(3)); 

D12 = D21; 

D= [D11 D12;D21 D22]; 


C12 = m2 * r1 * sin(x(3)); 
C-[-C12*x(4) = C12 > (x(2) + x(4));C12 * x(1) 0]; 


gl = (m1 + m2) * r1 * cos(x(3)) * n2* r2* cos(x(1) * x(3)); 
g2 = m2 * r2 * cos(x(1) * x(3)); 
G-[g1;g2]; 


Fr-[10*x(2) *3* sign(x(2));10 * x(4) * 3* sign(x(4))]; 


tol = [u(1) u(2)]*; 
S= inv(D) * (tol ~- Cx* [x(2);x(4)] — G- Fr); 


sys(1) = x(2); 

sys(2) = S(1); 

sys(3) = x(4); 

sys(4) = S(2); 

function sys = mdlOutputs(t, x, u) 

Fr-[10*x(2) *3* sign(x(2));10 * x(4) * 3* sign(x(4))]; 


sys(1) » x(1); 
sys(2) = x(2); 
sys(3) = x(3); 
sys(4) = x(4); 
sys(5) » Fr(1); 
sys(6) = Fr(2); 


(4) 作 图 程序 : chap4. 4plot. m. 


close all; 


figure(1); 

subplot(211); 

plot(t, yd1(:,1), 'r',t, y(:, 1), 'b', 'linewidth',2); 
xlabel('time(s)');ylabel('angle tracking of first link'); 
subplot(212); 

plot(t, yd2(:,1), 'r', t, y(:, 3), 'b', 'linewidth',2); 


第 4 章 ”机 械 手 模糊 自 适应 控制 P 85 


xlabel('time(s)');ylabel('angle tracking of second link'); 


figure(2); 

subplot(211); 

plot(t,0.3 * cos(t), 'r',t, y(:,2) , 'b', 'linewidth',2); 
xlabel('time(s)');ylabel('angle speed tracking of first link'); 
subplot(212); 

plot(t,0.3 * cos(t), 'r', t, y(:,4), 'b', 'linewidth',2); 
xlabel('time(s)');ylabel('angle speed tracking of second link'); 


figure(3); 

subplot(211); 

plot(t,y(:,5), 'r',t,u(:,3), 'b', 'linewidth',2); 
xlabel('time(s)');ylabel('f and fc'); 
subplot(212); 

plot(t,y(:,6), 'r',t,u(:,4), 'b', 'linewidth',2); 
xlabel('time(s)');ylabel('f and fc'); 


figure(4); 

subplot(211); 

plot(t,u(s,1),' r')z 

xlabel('time(s)');ylabel('control input of first link', 'llinewidth',2); 
subplot(212); 

plot(t,u(s:,2), 'r'); 

xlabel('time(s)');ylabel('control input of second link', 'linewidth',2); 


z265  Sin2 
仿真 实例 2: 针对 带 有 干扰 的 情况 , 取 干扰 项 为 r, 一 in d ,采用 基于 干扰 模糊 


补偿 的 机 械 手 控制 ,仿真 中 分 别 考 虑 带 鲁 棒 项 和 不 带 鲁 棒 项 两 种 情况 ,分别 取 M 王 1 和 
M= 王 2, 采 用 控制 律 式 (4. 77)、 和 鲁 棒 控 制 律 式 (4. 78) 及 自 适 应 律 式 (4. 790, HUM — 1, 


bus n A w= lo i ; ,仿真 结果 见 图 4-20 — H8 4-23, 


o 
2 
n2 


position tracking for link 1 


0 2 4 6 8 10 
time(s) 


speed tracking for link 1 


d" 2 4 6 8 10 


time(s) 


图 4-20 关节 1 的 角度 和 角速度 跟踪 
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N 
E 
号 04 
S 02 
EJ 
d 0 
总 
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N 
* 0.4 
P 0.2 
[^4 
E 0 
E 
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© 
8 -040 2 3 4 S 6 7 8 9 10 
d time(s) 
图 4-21 关节 2 的 角度 和 角速度 跟踪 
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图 4-22 双关 节 控 制 输 入 
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4-23 ”双关 节 摩 擦 及 其 补偿 
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基于 外 加 干扰 模糊 补偿 的 机 械 手 控制 的 仿真 程序 如 下 : 
(1) Simulink 主 程序 : chap4 5sim. mdl, 


To Workspace? 


(2) 控制 器 S PRA: chap4_5ctrl. m, 


function [sys,x0, str,ts] = MIMO Tong s(t,x,u, flag) 
switch flag, 
case 0, 
[sys, x0, str, ts] = ndlInitializeSizes; 
case 1, 
sys = ndlDerivatives(t,x,u); 
case 3, 
Sys = mdlOutputs(t,x,u); 
case (2,4,9) 
sys- []; 
otherwise 
error(['Unhandled flag = ',num2str(flag)]); 
end 


function [sys, x0, str, ts] = mdlInitializeSizes 
global nmnl nmn2 Fai 

nmnl = 5;nmn2 = 5; 

Fai= [nmnl 0;0 nmn2]; 

sizes = simsizes; 


sizes.NumContStates 


LU 
N 
* 
Un 
> 
A 


sizes. NumDiscStates 


I 
[e] 


sizes. NumOutputs 
sizes.NumInputs 
sizes.DirFeedthrough 


sizes. NumSampleTimes 


li 


LU 
Oro ux 
~ 


sys = simsizes(sizes); 
x0 = [0.1*0nes(2* 5^4,1)]; 
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str = [1; 

ts = [E 

function sys = mdlDerivatives(t, x, u) 
global nmnl nmn2 Fai 

qdi - u(1); 

qd2 - u(2); 

dqdl = 0.3 * cos(t); 

dqd2 = 0.3 * cos(t); 

dad = [dqdl dqd2]'; 


ddqd1 = - 0.3 * sin(t); 
ddqd2 = - 0.3 * sin(t); 
ddqd = [ddqdl ddqd2]'; 


q1 = u(3);dq1 = u(4); 
q2 = u(5);dq2 = u(6); 


for 11 = 171:5 
gs1 = - [ (q1 + pi/6 - (11- 1) * pi/12)/(pi/24)]^2; 
ul(11) = exp(gs1); 

end 

for 12:= 1:1:5 
gs2 = - [ (dq1 + pi/6 - (12- 1) * pi/12)/(pi/24)]^2; 
u2(12) = exp(gs2); 

end 

for 13= 1:1:5 
gs3 = - [ (q2 + pi/6 - (13- 1) * pi/12)/(pi/24)]^2; 
u3(13) = exp(gs3); 

end 

for 1472 1:1:5 
gs4- - [(dq2 + pi/6 - (14 - 1) * pi/12)/(pi/24)]^2; 
u4(14) = exp(gs4) ; 

end 


fsd- 0; 
fsu= zeros(5^4,1); 
for 1121:5 
for 122 1:5 
for 1321:5 
for 14=1:5 
fsu(5^3»(11-1)*5^2*(12-1) +5% (13-1) + 14)  u1(11) * u2(12) * u3(13) * u4(14); 
fsd = fsd + u1(11) * u2(12) x u3(13) * u4(14); 
end 
end 
end 
end 
fs = fsu/ (fsd + 0.001); 


el = q1 - qdl; 
e2 = q2 - qd2; 
e= [el e2]'; 
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del = dql - dqd1; 
de2 = dq2 - dqd2; 
de = [del de2]'; 


s-detFai*e; 
Gamal = 0.0001; 
Gama2 = 0.0001; 


S1 = - 1/Gamal * s(1) * fs; 

S2 = - 1/Gama2 * s(2) * fs; 

fori-1:1:5^4 
sys(i)-»S1(i); 

end 

for j 25°44 is1:2% 54 
sys(j) = S2(j- 5^4); 

end 


function sys = mdlOutputs(t, x, u) 
global nmnl nmn2 Fai 


ql = u(3);dql = u(4); 
q2 = u(5);dq2 = u(6); 


rl=1;r2=0.8; 
mi = 1;m2= 1.5; 


D11 = (ml + m2) * r1^2 + m2 * r2^2 +2 * m2 * r1 * r2 * cos(q2); 
D22 = m2 * r2^2; 

D21 = m2 * r2^2 + m2 * r1 * r2 * cos(q2); 

D12 = D21; 

D= [D11 D12;D21 D22]; 


C12 = m2 * r1 * sin(q2); 
C-[-C12*dq2 - C12 * (dql + dq2) ;C12 * q1 0]; 


gl = (m1 + m2) * r1 * cos(q2) * m2 * r2 * cos(ql * q2); 
g2 = n2 * r2 * cos(q1 + q2); 
G= [g1;g2]; 


adi = u(1); 
qd2 - u(2); 
dqdl = 0.3 * cos(t); 
dad2 = 0.3 * cos(t) ; 
dad = [dqd1 dqd2]'; 


ddqdl = - 0.3 * sin(t); 
ddqd2 = - 0.3 * sin(t); 
ddad = [ddqd1 ddqd2]'; 


el = q1 - qdl; 
e2 = q2 - qd2; 


90 «| 机 器 人 控制 系统 的 设计 与 MATLAB 仿 真 ， 基 本 设计 方法 (第 2 版 ) 


e= [el e2]'; 

del = dql - dqdl ， 
de2 = dq2 - dqd2; 
de = [del de2]'; 


s-de*Fai*e; 
dqr = dqd - Fai * e; 
ddar = ddqd - Fai * de; 


fori-21:1:5^4 
thtal(i,1)7x(i); 

end 

fori-21:1:5^4 
thta2(i,1) 7 x(i-*5^4); 


end 

forli-21:1:5 
gsl = - [ (q1 + pi/6 - (11 — 1) * pi/12)/(pi/24)]^2; 
ul(11) = exp(gs1); 

end 


for 12= 1:1:5 
gs2= - [(dql + pi/6 - (12 - 1) * pi/12)/(pi/24)]^2; 
u2(12) = exp(as2) ; 

end 

for 132 1:1:5 
gs3 = -[(q2 + pi/6 - (13- 1) * pi/12)/(pi/24)]^2; 
23(13) = exp(gs3); 

end 

for 14-7 1:1:5 
gs4= - [(dq2 + pi/6 - (14 - 1) * pi/12)/(pi/24)]^2; 
u4(14) = exp(gs4) ; 

end 


fsd= 0; 
fsu- zeros(5^4,1); 
for1121:5 
for 1221:5 
for 1321:5 
for 14= 1:5 
fsu(5^3* (11-1) *5^2*(12-1) *5* (13-1) * 14) - u1(11) * u2(12) * u3(13) * u4 (14); 
fsd- fsd + ul(ll) * u2(12) * u3(13) * u4(14); 
end 
end 
end 
end 
fs = fsu/ (fsd + 0.001); 


Fp(1) = thtal' * fs; 
Fp(2) = thta2' * fs; 


KD = 10 * eye(2) ; 
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W- [0.2 0;0 0.2]; 


M=1 
if M==1 

tol = D * ddqr + C * dqr + G+ 1 * Fp'- KD* s; $% (4.77) 
elseif M== 

tol = D * ddqr + C * dqr + G + Fp' - KDx s- Wxsign(s); % (4.78) 
end 


sys(1) = tol(1); 
sys(2) = to1(2); 
sys(3) = Fp(1); 
sys(4) = Fp(2); 


(3) 被 控 对 象 S AX: chap4 5plant. m, 


function [sys,x0, str, ts] = MIMO Tong plant(t, x, u, flag) 
switch flag, 
case 0, 
[sys, x0, str, ts] = ndlInitializeSizes; 
case 1, 
sys = mdlDerivatives(t,x,u); 
case 3, 
sys = mdlOutputs(t,x,u); 
case (2, 4, 9 ) 
sys = []; 
otherwise 
error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0, str, ts] = mdlInitializeSizes 
sizes - simsizes; 


sizes. NumContStates 


sizes. NumDiscStates 


sizes.NumOutputs 


sizes.NumInputs 
sizes.DirFeedthrough 
sizes.NumSampleTimes 


Li 
O o0» 00 x 
S 


Sys = simsizes(sizes); 


x0-[0000]; 
str-[]; 
ts-[]; 


function sys = mdlDerivatives(t, x, u) 
rl-1;r2720.8; 
ml = 1;m2 = 1.5; 


D11 = (m1 + m2) * r1^2 + m2 * r2^2 +2 * m2 * r1 * r2 * cos(x(3)); 
D22 = m2 * r2^2; 

D21 = m2 * r2^2 + m2 * r1 * r2 * cos(x(3)); 

D12 = D21; 

D= [D11 D12;D21 D22]; 


C12 = m2 * r1 * sin(x(3)); 
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C-[-C12*x(4) — C12 * (x(2) +x(4));C12 * x(1) 0]; 


gl= (m1 *m2) * r1 * cos(x(3)) * n2* r2 * cos(x(1) * x(3)); 
g2 = m2 * r2 * cos(x(1) * x(3)); 
G= [g1;g2]; 


told= [0.25 * sin(2* t);0.25* sin(2* t)]; 
F 7 told; 


tol- [u(1) u(2)]'; 
S= inv(D) * (tol ~ Cx* [x(2);x(4)] - G- told); 


sys(1) = x(2); 

sys(2) = S(1); 

sys(3) = x(4); 

sys(4) = S(2); 

function sys = mdlOutputs(t, x, u) 

told= [0.25 * sin(2* t);0.25 * sin(2* t)]; 
F= told; 


sys(1) = x(1); 
sys(2) = x(2); 
sys(3) = x(3); 
sys(4) = x(4); 
sys(5) - F(1); 
sys(6) - F(2); 


(4) 作 图 程序 : chap4 5plot. m. 


close all; 


figure(1); 

subplot(211); 

plot(t,ydl(:i,1),"r',t,y(:,1), b'); 
xlabel('time(s)');ylabel('position tracking for link 1'); 
subplot(212); 

plot(t, yd1(:,2), 'r',t, y( :, 2), 'b"); 
xlabel('time(s)');ylabel('speed tracking for link 1'); 


figure(2); 

subplot(211); 

plot(t,yd2( :,1), '£",£,y( :,3),, 'b/); 
xlabel('time(s)');ylabel('position tracking for link 2'); 
subplot(212); 

plot(t,yd2(:,2), '£',t,y(:,4), 'b'); 
xlabel('time(s)');ylabel('speed tracking for link 2'); 


figure(3); 

subplot(211); 

plot(t,u(:,1), 'r*); 
xlabel('time(s)');ylabel('control input of link 1'); 
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subplot(212); 
piot(t;u(:,2) ey 
xlabel('time(s)');ylabel('control input of link 2'); 


figure(4); 

subplot(211); 

plot(t, y(:,5), 'r', £,0(:, 3), Dy 
xlabel('time(s)');ylabel('friction 1 and its compensator'); 
subplot(212); 

plot(t,y(:;6)."r',E,u(5,4), b); 
xlabel('time(s)');ylabel('friction 2 and its compensator'); 


仿真 实例 3: 针对 带 有 基于 摩擦 、 外 加 干扰 模糊 补偿 的 机 械 手 控制 的 情况 ,采用 基于 模 
糊 补偿 的 机 械 手 控制 ,仿真 中 分 别 考 虑 带 鲁 棒 项 和 不 带 鲁 棒 项 两 种 情况 ,分 别 取 M 王 1 和 
M 王 2, 采 用 控制 律 式 (4. 83)、 自 适应 律 式 (4. 84) 3X (4. 85) 及 鲁 棒 控 制 律 式 (4. 86)。 取 
3q 1 十 0. 2sgnq , i si 
M—LBHIOTOS FG = » MEL Dl sun Mk= " 中 ， 
2q 4-0. 2sgnq ， 0.1 sin201 0 10 


w= f | 。 采 用 控制 律 式 (4. 830 ,仿真 结果 见 图 4-24 一 图 4-26, 
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图 4-24 双关 节 角 度 跟踪 


time(s) 


图 4-25 双关 节 摩 擦 及 其 补偿 
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20 =r 


control input 1 
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time(s) 


control input 2 
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E 4-26 ”双关 节 控 制 输入 


基于 摩擦 、 外 加 干扰 和 负载 变化 模糊 补偿 的 机 械 手 控制 的 仿真 程序 如 下 : 
(1) Simulink 主 程序 : chap4_6sim. mdl。 


| la 
Deryalive [me ari: 


Clock To Workspace 


(2) 控制 器 S 函数 : chap4 6ctrl. m, 


function [sys,x0, str,ts] = MIMO Tong s(t,x,u,flag) 
switch flag, 
case 0, 
[sys, x0, str, ts] = ndlInitializeSizes; 
case 1, 
Sys = ndlDerivatives(t,x,u); 
case 3, 
sys = mdlOutputs(t,x,u); 
case {2,4,9} 
sys=[]; 
otherwise 
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error(['Unhandled flag = ',num2str(flag)]); 
end 


function [sys, x0,str,ts] = mdlInitializeSizes 
global nmnl nmn2 Fai 

nmnl = 5;nmn2 = 5; 

Fai= [nmnl 0;0 nmn2]; 


sizes = simsizes; 


sizes.NumContStates = 4*5°4; 
sizes.NumDiscStates = 0; 
sizes.NumOutputs = 4; 
sizes.NumInputs - 10; 
sizes.DirFeedthrough = 1; 
sizes.NumSampleTimes = 0; 


SyS 7 simsizes(sizes); 


x0 = [0.1*0nes(4*5^4,1)]; 
str = []; 
ts = [E 


function sys = ndlDerivatives(t, x,u) 
global nmnl nmn2 Fai 

qdl = u(1); 

qd2 = u(2); 

dqdl = 0.3 * cos(t); 

dqd2 = 0.3 * cos(t); 

dqd= [dqdl dqd2]'; 


ddqd1 = - 0.3 * sin(t); 
ddqd2 = — 0.3 * sin(t); 
ddqd = [ddqdl ddqd2]'; 


qi = u(3);dal = u(4); 
q2 = u(5);dq2 = u(6); 
ddql = u(9);ddq2 = u(10); 


for 11-2 1:1:5 
gsl = - [(a1 + pi/6 - (11- 1) * pi/12)/(pi/24)]^2; 
ul(11) = exp(gsl); 

end 

for 12-2 1:1:5 
gs2= - [(dq1 + pi/6 - (12- 1) * pi/12)/(pi/24)]^2; 
u2(12) = exp(gs2) ; 

end 

for 13-2 1:1:5 
gs3 = - [ (q2 + pi/6 - (13- 1) * pi/12)/(pi/24)]^2; 
u3(13) = exp(as3); 

end 

for 1421:1:5 
gs4 = - [(dq2 + pi/6 - (14 - 1) * pi/12)/(pi/24)]^2; 
u4(14) = exp(gs4) ; 

end 
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for 1521:1:5 
gs5= = [(ddql + pi/6 - (15 - 1) x pi/12)/(pi/24) ]^2; 
u5(15) = exp(gs5); 

end 

for 1672 1:1:5 : 
gs6= - [ (ddq2 + pi/6 - (16 - 1) * pi/12)/(pi/24)]^2; 
u6(16) = exp(gs6) ; 

end 


fsd= 0; 
fsu= zeros(5^4,1); 
for ll-21:5 
for 12721:5 
for 13=1:5 
for 14= 1:5 
fsu(5^3* (11-1) +5^2* (12- 1) *5* (13- 1) * 14) - u1(11) * u2(12) x u3(13) x u4(14); 
fsd- fsd + u1(11) * u2(12) * u3(13) * u4(14); 
end 
end 
end 
end 
fs = fsu/(fsd + 0.001); 


fsd1 = 0; 
fsul = zeros(5^4,1); 
for 11=1:5 
for 12=1:5 
for 15=1:5 
for 16=1:5 
fsul(5^3*(11-1) *5^2*(12-1) +5% (15-1) * 16) - u1(11) * u2(12) * u5(15) = u6(16); 
fsdl = fsd1 + u1(11) * u2(12) * u5(15) x u6(16); 
end 
end 
end 
end 
fsl = fsul/(fsdl-* 0.001); 


el = q1 - qd1; 
e2 = q2 — qd2; 
e= [el e2]'; 
del = dq1 - dqd1; 
de2 = dq2 - dqa2; 
de = [del de2]'; 


s-detFai*e; 
Gamall = 0.01;Gamal2 = 0.01; 
Gama21 = 0. 01;Gama22 = 0.01; 


S11 = - 1/Gamall * s(1) * fs; 
S12= = 1/Gama12 * s(2) * fs; 
S21 = - 1/Gama21 * s(1) * fs1; 
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S22 = - 1/Gama22 * s(2) * fs1; 

fori-1:1:5^4 
sys(i)-»S11(i); 

end 

for j25^4t*1:1:2* 5^4 
sys(j) = $12(j- 5^4); 

end 

for j22*5^4*1:1:3* 5^4 
sys(j) = S21(j— 2 * 5^4); 

end 

for je 33*5^4- 121:4 5^4 
sys(3) = S22(j— 3 * 5^4); 

end 


function sys = mdlOutputs(t, x, u) 
global nmni nmn2 Fai 

qdl = u(1); 

qd2 = u(2); 

dqdl = 0.3 * cos(t); 

dqd2 = 0.3 * cos(t); 

dad = [dqdl dqd2]'; 


ddqdl = —0.3* sin(t); 
ddqd2 = —0.3 * sin(t); 
ddqd = [ddqdl ddqd2]'; 


ql = u(3);dq1 - u(4); 
q2 7 u(5);dq2 = u(6); 
ddql = u(9);ddq2 = u(10); 


for 11-21:1:5 
gsl = -[(q1 + pi/6 - (11 - 1) * pi/12)/(pi/24)]^2; 
ul(11) = exp(gsl); 

end 

for 1221:1:5 
gs2- - [(dql + pi/6 - (12 - 1) * pi/12)/(pi/24)]^2; 
u2(12) = exp(gs2); 

end 

for 1321:1:5 
gs3- - [ (q2 + pi/6 - (13 - 1) * pi/12)/(pi/24)]^2; 
u3(13) = exp(gs3); 

end 

for 14-2 1:1:5 
gs4 = - [(dq2 + pi/6 - (14 - 1) * pi/12)/(pi/24)]^2; 
u4(14) = exp(gs4); 

end 


for15251:1:5 
gs5 = - [(ddq1 + pi/6 - (15 - 1) * pi/12)/(pi/24)]^2; 
u5(15) = exp(gs5); 

end 
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for 16 = 1:1:5 
gs6 = - [ (ddq2 + pi/6 - (16 - 1) * pi/12)/(pi/24)]^2; 
u6(16) = exp(gs6) ; 

end 


fsd= 0; 
fsu= zeros(5^4,1); 
for 11=1:5 
for 12= 1:5 
for 13=1:5 
for 14=1:5 
fsu(5^3*x(11-1)*5^2*(12-1)*5* (13-1) * 14) =u1(11) * u2(12) * u3(13) * u4(14); 
fsd= fsd + u1(11) * u2(12) * u3(13) * u4(14); 
end 
end 
end 
end 
fs = fsu/(fsd + 0.001); 


fsdl = 0; 
fsul = zeros(5^4,1); 
for 1121:5 
for 12= 1:5 
for 15=1:5 
for 16= 1:5 
fsul(5^3x (11-1) +5^2*(12-1)+5» (15- 1) * 16) 2 u1(11) * u2(12) * u5(15) * u6(16); 
fsdl- fsdl * u1(11) * u2(12) * u5(15) * u6(16); 
end 
end 
end 
end 
fsl = fsul/(fsd1 + 0.001); 


el = ql - qd1; 
e2 = q2 - qd2; 
e= [el e2]'; 
del = dql — dad; 
de2 = dg2 - dqd2; 
de = [del de2]'; 


s-detFai*e; 
dar = dqd - Fai*e; 
ddqr = ddqd - Fai * de; 


for i-1:1:5^4 
thtal(i,1) = x(i); 
end 
fori-21:1:5^4 
thta2(i,1)-7 x(i-*5^4); 
end 
fori-21:1:5^4 
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nesr 


thta3(i,1) =x(i+2x5°4); 
end 
fori-1:1:5^4 

thta4(i,1) x(i*3*5^4); 
end 
s /LIIdM7MM LM TM MGM 
rl-1;r2-0.8; 
ml=1;m2=0.8; 


D11 = (m1*m2)*r1^2*m2*r2^242*m2*rl1*r2*cos(q2); 
D22 = m2 * r2^2; 

D21 = m2 # r2^2 + m2 * r1 * r2 * cos(q2); 

D12 = D21; 

D= [D11 D12;D21 D22]; 


C12 = m2 * r1 * sin(q2); 
C= [- C12 * dq2 - C12 * (dql * dq2);C12 * ql 0]; 


gl = (ml + m2) * r1 * cos(q2) + m2 * r2 * cos(q1 + q2); 
g2 = m2 * r2 * cos(ql + q2); 
G= [g1;g2]; 


Fp11 = thtal' * fs; 
Fp12 = thta2' * fs; 
Fp21 = thta3' * fsl; 
Fp22 = thta4' * fs1; 


Fpl- [Fpll Fpl2]'; 
Fp2 = [Fp21 Fp22]'; 


KD= 10 * eye(2); 
W= [2 0;0 2]; 


tol =D* ddqr + C * dqr+G+Fpl+Fp2— KD* s; % (4.83) 
elseif M==2 

tol = D * ddqr + C * dqr + G+ Fpl + Fp2 - KD * s- W=* sign(s); % (4.86) 
end 
sys(1) = tol(1); 
sys(2) = tol(2); 
sys(3) = Fp1(1) + Fp2(1); 
sys(4) = Fp1(2) + Fp2(2); 


(3) 被 控 对 象 S 函数 : chap4 6plant. m, 


function [sys, x0, str, ts] = MIMO Tong plant( t, x, u, flag) 
Switch flag, 
case 0, 

[sys, x0, str, ts] = mdlInitializeSizes; 


case 1, 
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mem 


sys = ndlDerivatives(t,x,u); 
case 3, 

sys = mdlOutputs(t,x,u); 
case (2, 4, 9} 

sys = []; : 
otherwise 

error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0, str, ts] = ndlInitializeSizes 


sizes = simsizes; 


sizes. NumContStates 


sizes.NumDiscStates  - 


4 
0 
sizes.NumOutputs = 6; 
Sizes.NumInputs = 4 
sizes.DirFeedthrough = 0 
sizes.NumSampleTimes - 0 
Sys = sinsizes(sizes); 
x0-[0000]; 
str-[]; 
ts-[]; 
function sys = ndlDerivatives(t, x,u) 
rl-21;r2720.8; 
ml = 1;m2= 1.5; 


D11 = (ml + m2) * r1^2 + m2 * r2^2 +2 * m2 # rl *r2*cos(x(3)); 
D22 = m2 x r2^2; 

D21 = m2 * r2^2 + m2 * r1 * r2 * cos(x(3)); 

D12 = D21; 

D= [D11 D12;D21 D22]; 


C12 = m2 * r1 * sin(x(3)); 
C-[-C12*x(4) -C12* (x(2) * x(4));C12 * x(1) 0]; 


g1 = (m1 + m2) * r1 * cos(x(3)) * n2* r2* cos(x(1) * x(3)); 
g2 = m2 * r2*cos(x(l) *x(3)); 
G= [g1;g2]; 


Fr-[3*x(2) *0.2* sign(x(2));2* x(4) * 0.2* sign(x(4))]; 
told= [0.05 * sin(20* t);0.1* sin(20 * t)]; 
F= told + Fr; 


tol= [u(1) u(2)]'; 
S- inv(D) * (to1- C* [x(2);x(4)] - G- F); 


sys(1) = x(2); 
sys(2) = S(1); 
sys(3) = x(4); 
sys(4) = S(2); 


function sys = mdlOutputs(t,x,u) 
Fr-[3*x(2) *0.2* sign(x(2));2* x(4) * 0.2* sign(x(4))]; 
told= [0.05 * sin(20 * t) ;0.1* sin(20 * t)]; 
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F = told + Fr; 


sys(1) = x(1); 
sys(2) = x(2); 
sys(3) = x(3); 
sys(4) = x(4); 
sys(5) = F(1); 
sys(6) - F(2); 


(4) 作 图 程序 : chap4. 6plot. m. 


close all; 


figure(1); 

subplot(211); 

plot(t, ydl( z,1),.'z*, t, y( 5, 1), 'b*) s 
xlabel('time(s)');ylabel('position tracking 1'); 
subplot(212); 

plot(t,yd2( :,1),. 'x',£,9( :,3), 'b'); 
xlabel('time(s)');ylabel('position tracking 2'); 


figure(4); 

subplot(211); 

plot(t,y(i,5Y, v, €, 2,3), *b'); 
xlabel('time(s)');ylabel('fl and fpl'); 
subplot(212); 

plot(t,9g(:,6), E £,u( 5,4), !b')s 
xlabel('time(s)');ylabel('f2 and fp2'); 


figure(3); 

subplot(211); 

plot(t,;u[:,1), 'x'); 
xlabel('time(s)');ylabel('control input 1'); 
subplot(212); 

plot(t,;u(:,2), '£'); 
xlabel('time(s)');ylabel('control input 2'); 


4.5 基于 线性 矩阵 不 等 式 的 单 级 倒立 摆 T-S 模糊 控制 


单 级 倒立 摆 系 统 是 一 种 特殊 的 单 力 臂 机 器 人 被 控 对 象 ,是 一 个 复杂 的 非 线性 的 、 不 确定 
系统 ,其 控制 器 的 设计 应 保证 有 良好 的 鲁 棒 稳定 性 。 

线性 矩阵 不 等 式 (Linear Matrix Inequality, LMI) 是 控制 领域 的 一 个 强 有 力 的 设计 工 
具 。 许多 控制 理论 及 分 析 与 综合 问题 都 可 简化 为 相应 的 LMI 问题 ,通过 构造 有 效 的 计算 机 
算法 求解 。 

随 着 控制 技术 的 迅速 发 展 , 在 反馈 控制 系统 的 设计 中 , 常 需要 考虑 许多 系统 的 约束 条 
件 ,例如 ,系统 的 不 确定 性 约束 等 。 在 处 理 系统 鲁 棒 控制 问题 以 及 其 他 控制 理论 引起 的 许多 
控制 问题 时 ,都 可 将 所 控制 问题 转化 为 一 个 线性 矩阵 不 等 式 或 带 有 线性 和 矩阵 不 等 式 约束 的 
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最 优化 问题 。 目 前 线性 矩阵 不 等 式 技术 已 成 为 控制 工程 .系统 辨识 ` 结 构 设 计 等 领域 的 有 效 
工具 。 利 用 线性 矩阵 不 等 式 技术 来 求解 一 些 控制 问题 ,是 目前 和 今后 控制 理论 发 展 的 一 个 


重要 方向 。 
采用 T-S 模糊 系统 进行 非 线 性 系统 建 模 的 研究 是 近年 来 控制 理论 的 研究 热点 之 一 。 


实践 证 明 , 具 有 线性 的 Takagi-Sugeno 模糊 模型 以 模糊 规则 的 形式 充分 利用 系统 局 部 信息 
和 专家 控制 经 验 , 可 任意 精度 通 近 实际 被 控 对 象 。T-S 模糊 系统 的 稳定 性 条 件 可 表述 成 线 
性 矩阵 不 等 式 的 形式 ,基于 T-S 模糊 模型 的 非 线性 系统 鲁 棒 稳 定 和 自 适 应 控制 的 研究 是 控 
制 理 论 研 究 的 热点 。 


4.5.1 基于 LMI BN T-S 型 模糊 系统 控制 器 的 设计 


针对 7 个 状态 变量 om. 个 控制 输入 的 连续 非 线性 系统 ,其 T-S 型 模糊 模型 可 描述 为 以 
F or 条 模糊 规则 : 
LU] i. 
IF x, (2) is Mj and z: (€) is M; and «e x, tt) is M, (4. 87) 
THEN z(G)sA,x0)-FB;(), i=1,2, ,7 
Hope; 为 系统 的 第 j 个 状态 变量 ,M ;为 第 i 条 规则 的 第 j 个 隶属 函数 ,x(1) 为 状态 向 
R.xG)—[r;,Q) z4) = z, (1)]"ER",u (it) 为 控制 输入 向 量 ,wu CD m [ui CO 
ay) c u GONT ER A ER ,BERY, 
根据 模糊 系统 的 反 模糊 化 定义 ,由 模糊 规则 (4. 87) 构 成 的 模糊 模型 总 的 输出 为 


X w, [Ax GO 4- Bult)] 
多 - 
>w, 
i=1 


其 中 ,vw, 为 规则 i 的 隶属 函数 ,zw = [| Mi (x02)), 以 4 条 规则 为 例 ,规则 前 提 为 rz, ,k=1， 
k=] 
i=1;2;3s4, W «o, =M} Cr w; =M} (ri) w =M] (z1 )sw =M lri) 
针对 每 条 T-S 模糊 规则 ,采用 状态 反馈 方法 ,可 设计 以 下 条 模糊 控制 规则 。 
控制 规则 i. 


(4. 88) 


IF z; G0) is Mi and xz; (0) is M; and +" xr,G) is M, (4. 89) 
THEN aGbeREaük. TSI brer 
并 行 分 布 补偿 (Parallel Distributed Compensation, PDC) 方 法 是 由 Sugeno 等 所 提出 的 
一 种 基于 模型 的 模糊 控制 器 设计 方法 ,该 方法 的 稳定 性 分 析 由 文献 L4j 给 出 ,适用 于 解决 基 
于 T-S 模糊 建 模 的 非 线 性 系统 控制 问题 中 。 
关于 利用 LMI 方法 设计 基于 T-S 模糊 建 模 的 非 线 性 系统 控制 问题 ,文献 L6,7] 有 许多 
描述 。 根 据 模糊 系统 的 反 模 糊 化 定义 ,针对 连续 非 线 性 系统 式 (4. 87) ,根据 模糊 控制 按 规则 
式 (4. 89), 采 用 PDC 方法 设计 T-S 型 模糊 控制 絮 为 


Dw K xa) 
u(t) == (4. 90) 


w 
j=] 


J 
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4.5.2 LM 不 等 式 的 设计 及 分 析 
定理 4.1 . 存在 正定 阵 8 , 当 满 足下 面条 件 时 ,T-S 模糊 系统 (4. 87) 渐 进 稳定 。 


--VIB; -- B,V, <0, TURNS 
QA! --AQQ--QA] --A,Q -VIB! --B,V; -VIBI --BjV, <0, i<j<r (4.91) 
Q—P'0 


JtB.V, Kg. HI K, —V,Q ^ —V,P.V, —K,Q. HI K,—V;Q ' —V,P. 
定理 4.1 的 给 出 见 文献 L8]。 根 据 式 (4.91) ,利用 LMI 方法 可 求 出 控制 器 式 (4. 900 ff 
增益 K,。 下 面 给 出 定理 4. 1 的 具体 分 析 过 程 。 
取 Lyapunov 函数 
VD) 一 了 xzTPx 


HoP, ERE P 为 正定 对 称 和 矩阵 。 


则 有 
. ETT pes la li Tp 
Vit) 2 Px ix Px)—-Xx Px 4- 2* Pý 
[Este : [Est 
一 = - > Pe 十 村 7 P = - > 
| Bm | | E. ] 
i-1 i=l 
将 控制 律 式 (4. 90) 代 入 上 式 , 可 得 
r jm r w,K;x 
Pw, [Ax +B, 过- Sw, Aiz 4- B, 二 
vo -14 i Dw, Px +ga Py Dw, 
Z j-1 j=1 
am | han | 
1 i=1 
r r r T r r r 
1 Yu [wA E» 1 | w| DA +B, Dk 
eL. ger ei PP m. 一 一 
QNM | ww | 
i-1 j=1 i=l j=1 
Sia gs Ge +BKx)| i S Las T EER T A 
- e — Px+sxP - 
| 3 2j 21ww, 
i=1 j=1 ， i=1 j=1 
; 357 aum jn CL, d- BUR, pp TEE T A 
i=] j-1 i=] j=1 


= Px + —x'P 


2 E ut 2 E. 
353/22 DYD ww, 


i=1 j21 1 一 ] j=1 
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S d ndi (A; T B,K,)' 31 Yr, (A, 二 BK,) 
= 一 一 Px 十 了 x 卫 二 -一 x 
2j 2 ww; | 2j 2 wiw; 
i=1 j21 i=] j=l 


r 


, | w,w;[(A, + B,K;)' P +P(A; Pza] 
T] i=l j=l 
— x | 


x 


X D ww, 


i=] j=1 


di 
分 别 考 虑 ;一 ) Hic; 两 种 情况 ,将 式 V(Gz) 展 开 , 得 


X D ww [(A; -- BK;)'P +P(A; - BK))]— >) waw,[(A, -BK;))' P +P(A; 4- BK] + 


i=} j=l i=j=1 


> ww LA; + B.K;)' P +P(A; - BK.) ] + 
i<j 


> ww, [(4; 4- BK,)'P +P(A, -- BK,)] 


i>j 


(4. 92) 
E: VA r—2 为 例 ,可 得 如 下 展开 


2 2 r r r 
M D ww, = > ， w;w; 十 > ww; 十 > ww, 一 WII T w;ws-uw,w;-uw;w, 


i=] j71 i=j=1 ij 


由 于 i 和 j 交换 不 影响 结果 , 则 
X ww [(A; BRITPHPA, - BK,) ] ^ X w;w;[(4; +B,K;)"P +P(A, +B,K,)] 
从 而 


S Sl ww,[(A; 4- BK;)'P +P(A; -- B.K;)] 


i=1 j=1 


= M ww,[(A, -- B,K.)'P +P(A; - BK.) ] 


i-j-1 


S ww [Ah; 4- BK,)' P +P(A; - BK) ] - 2  w;w,[(A; -- B,K,.)'P +P(A, +B,K,)] 


= M wyw,[(A, -- B.K,.)'P +P(A; - BK.) ] + 
i=j=1 


X ww [(CA; +B:K,)+ (A, +B,K,))'P - PCCJA, +B,K,) + (A, +B,K;))] 


则 


S wiw,[(A, + B,K.)*P +P(A; 4- BK.) ]x 十 


Pow 
i—-j-1 
> > LU 


i=} j—1 


: 1 
V) * 
gt 
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1 
2 


x 


N ; 
i > ww, [((A; +B:K,) + (A; +B,K,;))"P + 


r r 
i<j 
2; wiw, 


i=l j=l 
P((A; +B;K;) + (A; +B;,K;))]x 
4 G; — (A; -B,K;) CA, - B,K,) n] f 


. T... om 1 z z 
Ves" 3 ww,[(A, + B,K.)'P 4- P(A, +B,K;)]x + 
39Yww jl 
i=1 j=1 
» 1 : i 
P» ——— — M ww [GIP -- PG, ]x 
2j 21wow, ^ 
i=1 j=1 
则 当 满 足 如 下 不 等 式 


(A; +B;K;)'P+P(A;+B;K;)<0, i=j=1,2y°,r 
GiP+PG, <0, i<jSr 
AVA). 
由 式 (4. 92) 可 见 , 当 V 夺 0 Bf x=0, 483 LaSalle 不 变性 原理 ,1 一 00 时 ,x 一 0。 


4.5.3 不 等 式 的 转换 


首先 考虑 (4; 十 B,K,)'P 二 P(A; 十 B,K,) 二 0,i==j 二 1,2,…,r。 取 0Q==P ', 则 0 也 是 
正定 对 称 矩 阵 , 令 V; 二 K,Q , 则 
A'P+KIBIP+PA;+PB,K,;<0 
上 式 中 的 每 个 式 子 两 边 分 别 乘 以 P ,得 
PA] +P KIB] tA; P BRP <0 
即 
QAI -ViB;| *A,Q--B,V,-—0 
即 
QA? --A,Q -VjB; —-B,V,—0 
然后 考虑 GIP 十 PG,; «0,G; —(A; -B,K)) (JA; -B;K) ijr, ROSP, WO 
也 是 正定 对 称 和 矩阵 。 令 VV; 一 K,Q ,Vj 一 K,Q , 即 
(A; HBE - (A; TBER PHP(A BK) - CA; - B,K,0) 0 
上 式 中 的 每 个 式 子 两 边 分 别 乘 以 己 ,并 考虑 0 一 8 ,得 
Q' CA, -B,K,) - (A, +B,K:))"+( (A; -B,K)) - (A; -BjK,)))Q 0 


即 
(4,O 十 BKO 十 AQ 十 BiKIO) 二 AIO T B,K;Q--A;Q- B,K,Q—0 
从 而 得 l 
(A,Q-- B,V; -A,Q-B,V,.)' -A,Q-B,V, -A,Q-B,V, «0 
即 


QA? -A,Q-Q AT 3-A,Q -VIB; 4-B,V, -V7B; +B,V;<0 
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4.5.4 ”LMI 设计 实例 说 明 


实例 1. 如 模糊 系统 由 2 条 模糊 规则 ,xr 二 2, 有 i 二 1,2, 则 LMI 不 等 式 如 下 : 
QAI -A,Q- VI BI -B,V, <0 
QAI -A,Q -VIBI -B,V, <0 
针对 i 二 j mr A i=l, j —2.2 条 规则 隶属 函数 相互 作用 , 则 LMI 不 等 式 如 下 : 
QAI A,Q-Q A? -A,Q--VI BL -B,V, +V BI +B,V, <0 
写成 MATLAB 程序 如 下 : 
L1 =Q» A1l'+A1*Q+V1'* B1'+ B1 * V1; 
L2- Q* A2'+ A2 * Q+ V2' * B2' + B2 * V2; 
L3 = Q* AI'F AT* Q- O* A2'+ A2% Q- V2' * B1'+ B1 * V2 - VI' * B2' + B2* V1; 
实例 2. 如 模糊 系统 由 4 条 模糊 规则 ,x 二 4。 
考虑 单条 规则 ,有 i 二 1,2,3,4, 则 可 构造 4 条 LMI 不等式 如 下 : 
QAT +A Q -VIBL -B,V, <0 
QA; - AjQ-Vi Bi -B,V, <0 
QA; -A,Q -ViBi -B,V, <0 
QA; +A,Q t-ViBi -B,V, <0 
写成 MATLAB 程序 如 下 : 


L1-Q*A1'tA1*Q-VI'*B1'-*B1*V1; 
L2 = Q * A2' + A2 * Q+ V2'* B2' + B2 * V2; 
L3 = Q * A3' + A3 * Q+ V3' * B3' + B3 * V3; 
LA =Q* A4'+ A4 * Q+ V4' * B4' + B4 * V4; 


针对 i 过 j 达 7 , 则 可 构造 如 下 LMI 不 等 式 。 根 据 @ A7 -A,Q-Q AT —-A,Q —-VIB] — 
B,V; -ViB; 十 B;V; 二 0, 可 能 存在 的 不 等 式 如 下 : i 二 1,j —2,1—1,7—3.i—1,]—4; i—2, 
j=3,i=2,j=4; i 二 3,j 二 4。 设 计 LMI 不 等 式 时 ,应 考虑 隶属 函数 i 和 隶属 函数 ) 是 否 有 
隶属 函数 相互 作用 。 

考虑 第 3 条 规则 的 隶属 函数 和 第 4 条 规则 的 隶属 函数 相互 作用 , 即 i 二 3,j — 4. BEI 
的 LMI 不 等 式 如 下 : 

QAi--A,Q--QAT--A,Q--VI BI -B,V,-VI BI -B,V,—0 

写成 MATLAB 程序 如 下 : 


L-Q*A3't A3* Qt Q* A4' t AA * Q+ VA' « B3' * B3 * VA + V3' * B4' + BA * V3; 


4.5.5 单 级 倒立 摆 的 T-S 模型 模糊 控制 


倒立 摆 系 统 的 控制 问题 一 直 是 控制 研究 中 的 一 个 典型 问题 。 控 制 的 目标 是 通过 给 小 车 
底座 施加 一 个 力 u (控制 量 ) ,使 小 车 停留 在 预定 的 位 置 ,并 使 摆 不 倒 下 , 即 不 超过 预先 定义 
好 的 垂直 偏离 角度 范围 。 

单 级 倒立 摆 模 型 为 
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1 
| . | gsinr,—a mix;sin(2x,)/2 —a ucosr, (4. 93) 
in 4L/3 — a mlcos x, 
其 中 , x, 为 摆 的 角度 , x, 为 摆 的 角速度 ,2! WERK, u 为 加 在 小 车 上 的 控制 输入 ， 
a— prz M Ron 分 别 为 小 车 和 摆 的 质量 。 
1. 基于 2 条 模糊 规则 的 设计 


根据 倒立 摆 模 型 式 (4.93) 可 知 , 当 阅 一 0 RF, sinr, 2r, s coss, >l; rı >g sinr 


xr. ,由 此 可 得 以 下 两 条 T-S 型 模糊 规则 : 


规则 1; IF c, G) is about 0,THEN x()=Aix(t)+Bu(t); 
元 


2 


0 1 0 0 1 
RUBIA, = g 0 : B, P -= Q sÅ; = 2g 0 9 
4L/3—aml 4L/3—aml x(41/3—amlf" ) 


0 
" aß | ,8 一 cos(88 ) 。 
4L/3—amlf" 
根据 4.5.4 节 实 例 1. (8) vr PE B ZX FEX ER C n] Ao ON 
QAI +A, Q —-VI Bi - ByV, <0, 
QAI - AjQ -VIBI 4- B.V, <0, 
QA - AiQ-QA? - AQ -ViBI -B,V, -VI Bi - B,V, <0 
Q-—P?l0 
HR,K -—V,P.K,—V,P.i—1,2, 
针对 上 述 线 性 矩阵 不 等 式 ,可 采用 MATLAB 的 LMI 工具 箱 进 行 求解 。 针 对 倒立 摆 模 
型 , 取 g 一 9.8my/s: , 摆 的 质量 罗 王 2.0kg, 小 车 质量 M —8. 0kg.2/— 1. 0m, 
根据 倒立 摆 的 运动 情况 ,设计 2 条 模糊 控制 规则 : 
规则 1: IF zr, G) is about 0 THEN u=K;x(t) 


规则 2; IF a, (is about +5 (|x, |): THEN x Q)—A;,xG)--B,uG)., 


规则 2; IF a, GO is aboutt z (|x, CO | X.) THEN u —K;x() 


采用 PDC 方法 ,根据 式 (4.90) ,设计 基于 T-S 型 的 模糊 控制 器 为 
u-—uw;G )KxG)-- w;G K;x(G) 
Etr, w tw, =l, 
根据 倒立 摆 的 两 条 T-S 模糊 模型 规则 ,隶属 函数 应 按 图 4-27 进行 设计 。 仿 真 中 采用 三 


角形 隶属 函数 ( 见 图 4-28) 实 现 摆 角 度 x (1) 的 模糊 化 。 摆 角 初 始 状态 为 E o . 


采用 LMI 求 解 工 具 箱 -YALMIP 工具 箱 ( 见 本 章 附 录 介 绍 ) ,控制 器 增益 的 LMIGK fit fe 
序 为 chap4 7LMI design. m, 求 解 线性 矩阵 不 等 式 , 求 得 Q,Vi,V,, 从 而 得 到 状态 反馈 增 
i$. K,—[2400.8 692.3]. K,—[5171.6 1515. 3]。 然 后 运行 Simulink 主 程序 chap4_7sim. 
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mdl, 仿 真 结果 如 图 4-29 和 图 4-30 所 示 。 


T x 
E bout 0 E 
about 2 abou about 2 
1 
0 XX 
一 88 0 88 (deg) 


图 4-27 ”模糊 隶属 函数 示意 图 


membership function 
e Ed o 
d CN oo 


e 
io 


-2 -1.5 -1 -05 0 0.5 1 1.5 


N 


图 4-28 仿真 中 的 模糊 隶属 函数 


-0.4 


angle and angle speed response 
i e S b NES 
— 0) O i o N A 


0 Q5 1 1$ 2 25 3 35 4 45 5 
time(s) 


4-29 角度 和 角速度 响应 


control input 
t3 EN o oo 三 
> Ll 2 $ E 
o e e S e e 


un 


0 1 2 3 4 
time(s) 


图 4-30 控制 输入 


第 4 章 “ 机 械 手 模糊 自 适应 控制 | 109 


仿真 程序 如 下 : 
(1) 基于 LMI 的 控制 器 增益 求解 程序 : chap4. 7LMI design. m, 


clear all; 
close all; 


g79.8;m-2.0;M-8.0;170.5; 
a= l/(m-*M);beta = cos(88 * pi/180); 


al-4*1/3-a*m*1; 

A17 [0 1;g/al 0]; 

Bl-[0 ; -a/al]; 
a2-4*]1/3-a*m*1* beta^2; 
A2- [0 1;2* g/(pi * a2) 0]; 
B2- [0; - a * beta/a2]; 


Q= sdpvar(2,2); 
V1 = sdpvar(1,2); 
V2 7 sdpvar(1,2); 


L1-0Q*Al1'*Al1*Q-*Vl1'*Bl'*Bl*VIl; 
L2 = Q * A2' + A2 * Q+ V2' x B2' + B2 * V2; 
L3-2Q* A1l1'+ A1 * Q* Q* A2' A2 * Q+ V2' * B1'+ B1 x V2 + V1'* B2'+ B2* V1; 


F = set(L1«0) + set(L2«0) + set(L3«0) + set(Q» 0); 
solvesdp(F); * To get Q, V1, V2 


Q = double(Q); 
V1 = double( V1); 
V2 = double( V2); 


P= inv(Q); 

K1 =V1*P 

K2 = V2 * P 
save K_file K1 K2; 


(2) 隶属 函数 设计 程序 : chap4_7mf. m, 


clear all; 
close all; 
L1- = pi/2;I2 = pi/2; 
L-2L2-L1; 


h= pi/2; 
N= L/h; 
T=0.01; 


x=L1:T:L2; 
for i=1:N+1 
e(i)=L1+L/Nx (i—1); 
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end 
u7trimf(x,[e(1),e(2),e(3)]); * The middle MF 
plot(x,u, 'r', 'linewidth',2); 


for j=1:N 
if j--1 
u= trinf(x,[e(1),e(1),e(2) ]); * The first MF 
elseif j-- N 
u-trimf(x,[e(N), e(N* 1), e(N* 1)]); % The last MF 
end 
hold on; 
plot(x,u, 'b', 'linewidth',2); 
end 


xlabel('x'); ylabel( membership function'); 
legend('first rule', 'second rule"); 


(3) Simulink 主 程 序 : chap4. 7sim. mdl。 


chap4_7ctr chap4_7plant 


Clock To Workspace 


(4) 模糊 控制 SP: chap4 7ctrl. m, 


function [sys,x0, str,ts] = spacemodel(t, x,u, flag) 
switch flag, 
case 0, 

[sys, x0, str, ts] = ndlInitializeSizes; 
case 3, 

sys = ndlOutputs(t,x,u); 
case {2,4,9} 

sys-[]; 
otherwise 

error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0, str, ts] = ndlInitializeSizes 
Sizes - simsizes; 
sizes.NumContStates  - 
sizes.NumDiscStates  - 


sizes.NumOutputs - 


N- Oo o 


sizes.NumInputs - 
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nc 


sizes.DirFeedthrough = 1; 
sizes.NumSampleTimes - 1; 


sys = simsizes(sizes); 


x0 = [] 
SEE e [p 
ts = [00]; 


function sys = mdlOutputs(t, x, u) 
x= [u(1);u(2)]; 


load K file; 
uti = K1 * x; 
ut2 = K2 * x; 


L1 = - pi/2;L2 = pi/2; 
L-212-I1; 


N= 2; 
for i=1:N+1 
e(i)-L1*L/N*(i-1); 


end 
hl = trinf(x(1),[e(1),e(2),e(3)]); € The middle 
if x(1)«- 0 

h2 = trinf(x(1),[e(1),e(1),e(2)]); % The first 
else 

h2 = trinf(x(1),[e(2),e(3),e(3)]); * The last 
end 
* hl + h2 
ut= (h1 * utl + h2 * ut2)/(hl + h2); 
sys(1)= ut; 


(5) 作 图 程序 : chap4. 7plot. m, 


close all; 


figure(1); 
plot(t,x(:,1), 'r',t,x(:,2),, 'b'); 
xlabel('time(s)'); ylabel('angle and angle speed response"); 


figure(2); 

plot(t,ut(:,1), mys 

xlabel('time(s)'); ylabel('control input'); 

2. 基于 4 条 模糊 规则 的 设计 

为 了 能 在 大 范围 的 初始 角度 下 进行 控制 ,在 上 述 2 条 规则 的 基础 上 ,需要 增加 模糊 规则 
数量 。 


根据 倒立 摆 模 型 式 (4. 9TA on 2 e |) He sinn n B1 Totg 


B= cos88" , 则 cosx, — cos(180*— 88°) = —cos88' — —; 4 x, FE] sinz,—0.cosr,— — 1 Jl 
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au 


近似 有 之。 Ta 


由 此 可 得 以 下 另外 两 条 T-S 型 模糊 规则 。 


规则 3; IF c 0) is about 3-7 (|a, [>F ) THEN € Q)—A,4xG) Bu), 


规则 4: IF x, (t) is about x, THEN Xx (t)=A,x@)+B,u(t), 
0 1 


| eb dee 
g s Dg= Q A= D, 一 a o 
n(4l/3—amlß ) : 4L /3—amlff 9 o AL/3—aml 
根据 倒立 摆 的 运动 情况 ,设计 上 述 两 条 模糊 控制 规则 : 


规则 3. IF a, is about — zn -JTHEN «—K. x). 


其 中 A; = 


规则 4: IF x, G2 is about 士 xx THEN 4 —K,x(), 
如 图 4-31 所 示 ,为 具有 4 条 规则 的 隶属 函数 示意 图 ,隶属 函数 有 交集 的 规则 分 别 是 规 
则 1 和 2, 规 则 3 和 4, 带 有 交点 的 规则 才能 构成 一 个 不 等 式 。 故 针对 i 二 ;三 7 ,只 能 构造 2 
个 LMI, 根 据 4.5.4 节 的 实例 2, 所 对 应 的 LMI 不 等 式 如 下 : 
QAT +A Q+Q0A: - AQ -Vi Bi 十 BiV -VI BL -B,V, <0 
QAi AQ -QAT +4,0 +VIB] +B,V, -VIBI -B,V, <0 


图 4-31 模糊 隶属 函数 示意 图 


写成 MATLAB 程序 如 下 ， 


L5-20*Al1'tA1*0-*0Q*A2'*t A2*Q- V2' * B1'+ B1 * V2 * V1I' * B2' & B2 * V1; 

L6 = Q * A3' + A3* Q+ Q* A4' + A4 * Q+ VA' * B3'+ B3 * VA + V3' * B4' + BA * V3; 

采用 PDC 方法 ,根据 式 (4. 900 ,设计 基于 T-S 型 的 模糊 控制 器 为 

u —wiGi )KixG) d. ws; )K;x GO Tt war )K;xG) tw Ga KxG) 

根据 倒立 摆 的 两 条 T-S 型 模糊 规则 ,隶属 函数 应 按 图 4-32 进行 设计 。 仿 真 中 采用 三 角形 隶 
属 函 数 实现 摆 角 度 rz (Ci) 的 模糊 化 。 摆 角 初 始 状态 为 [r 0]. 

采用 LMI 求 解 工具 箱 一 YALMIP 工具 箱 ( 见 本 章 附录 介绍 ) ,控制 器 增益 的 LMI 
求解 程序 为 chap4_8LMI design. m, 求 解 线 性 矩阵 不 等 式 , 求 得 0,Vi ,Vs Va, Vi MA 
而 得 到 状态 反馈 增益 : KQ— [3301.3 969. diis K; Mees 3 1879.7],K,= 
[—6189.6 —1883.7];K,=[—3105.2 -—969.9], 运行 Simulink 主 程序 chap4_ 
8sim. mdl], 仿 真 结果 如 图 4-33 和 图 4-34 所 示 。 


1 


0.9 
g 08 
Š 07 
c 
2 06 
S 
[Es 
Eg 04 
v 
E 03 
0.2 
0.1 
0 
A 3 ^ 3 0 l 2 3 4 
X 
Bl 4-32 ”模糊 隶属 函数 
9 
E 
e 
g 
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i 
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E 
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time(s) 
图 4-33 角度 和 角速度 响应 
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0 
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图 4-34 控制 输入 
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wa 


仿真 程序 如 下 : 

(1) 基于 LMI 的 控制 器 增益 求解 程序 : chap4_8LMI design. m, 
clear all; 

close all; ' 


g=9.8;m=2.0;M=8.0;1=0.5; 
a= 1/(m + M);beta = cos(88 * pi/180); 


al-4*1/3-a*m*]l; 
A1 7 [0 1;g/a1 0]; 
B1 - [0 ; - a/al]; 


a2-4x*1/3-a*m*1* beta^2; 


A2- [0 1;2 * g/(pi * a2) 0]; 
B2 = [0; - a * beta/a2]; 


A3 = [0 1;2 * g/(pi * a2) 0]; 
B3 = [0;a * beta/a2]; 


A4 = [0 1;0 0]; 
B4 = [0;a/a1]; 


Q= sdpvar(2,2); 

V1 = sdpvar(1,2); 
V2 = sdpvar(1,2); 
V3 = sdpvar( 1,2); 
V4 = sdpvar (1,2); 


L1 =Q% A1'+ Al» Q+V1' x B1'+ B1 * V1; 
L2 = Q* A2' + A2 * Q+ V2' * B2' + B2 * V2; 
L3 = Q * A3' + A3 * Q + V3' * B3' + B3 * V3; 
L4 =Q* A4'+ A4 * Q* V4A' * B4' + B4 * V4; 


L5-2Q* AI'- AL*Q-* Q* A2' - A2 * Q-- V2' « B1' * Bl * V2 V1' * B2' « B2* V1; & from R1 and R2 
L6 = Q* A3' A3 * Q-- Q* A4! * Ad x Q^ VA' x B3' t B3 * V4 + V3' * BA' BA * V3; % from R3 and R4 


F= set(L1 <0) + set(L2« 0) + set(L3«0) + set(L4«0) + set(L5«0) +set(L6 <0) + set(Q» 0); 
solvesdp(F); * To get Q, V1, V2, V3, V4 


Q = double(Q) ; 
V1 = double(V1) 
V2 = double( V2); 
V3 = double( V3); 
V4 = double( V4) 


7 


7 


P= inv(Q); 


Kl1-V1*P 
K2 = V2 * P 
K3 = V3 * P 
K4 =V4*P 


save K_file K1 K2 K3 K4; 


(2) 隶属 函数 设计 程序 : chap4 8mf. m, 


clear all; 
close all; 
L1 = - pi;L2 = pi; 
L-2L2-L1; 


h= pi/2; 
N= L/h; 
T=0.01; 


x-L1:T:L2; 
for i=1:N+1 
e(i)=L1+L/N* (i-1); 
end 
figure(2); 
% hl 
hi-2trimf(x,[e(2),e(3),e(4)]);  %Rule 1:xl is to zero 
plot(x,hl, 'r', 'linewidth',2); 
* h2, Rule 2: xl is about *- pi/2,but smaller 


% if x<=0 
h2 = trimf (x, [e(2),e(2),e(3)]); 
hold on 
plot(x, h2, 'b', 'linewidth',2); 
% else 
h2 = trimf (x, [e(3),e(4),e(4)]); 
hold on 
plot(x, h2, 'b', 'linewidth',2); 
% end 


% h3, Rule 3: xl is about +- pi/2, but bigger 
%ifx<0 
h3 = trimf (x, [e(1),e(2),e(2)]); 
hold on; 
plot(x, h3, 'g', 'linewidth',2); 
% else 
h3 = trinf(x,[e(4),e(4),e(5)]); 
hold on; 
plot(x, h3, 'g', 'linewidth',2); 
% end 


% h4, Rule 4: xl is about +- pi 
%ifx<0 
h4 = trimf (x, [e(1),e(1),e(2)]); 
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hold on; 
plot(x, h4, 'k', 'Linewidth',2); 
* else 
h4 = trinf(x,[e(4),e(5),e(5)]); 
hold on; 
plot(x, h4, 'k', 'linewidth', 2); 
% end 


(3) Simulink 主 程序 : chap4_8sim. mdl。 


Clock — To Workspace 


(4) 模糊 控制 S 函数 ; chap4 8ctrl. m. 


function [sys,x0, str,ts] = spacemodel(t, x, u, flag) 
switch flag, 
case 0, 
[ sys, x0, str, ts] = mdlInitializeSizes; 
case 3, 
sys = ndlOutputs(t, x,u); 
case (2,4,9) 
sys - []; 
otherwise 
error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0, str, ts] = ndlInitializeSizes 


sizes = simsizes; 


sizes.NumContStates 
sizes.NumDiscStates = 


0 
0 
sizes.NumOutputs = 1; 
sizes.NumInputs = 2 
sizes.DirFeedthrough = 1 
sizes.NumSampleTimes = 1 


Sys 7 simsizes(sizes); 


x0 = [] 
str = []; 
ts = [00]; 


function sys = mdlOutputs(t, x, u) 
x= [u(1);u(2)]; 
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load K file; 
utl = K1 * x; 
ut2 = K2 * x; 
ut3 = K3 * x}; 
ut4 = K4 x x; 


L1 = - pi; L2 = pi; 
LsL2-LD1; 


h= pi/2; 
N-L/h; 


for i=1:N+1 
e(i)-Li-*L/N*(i-1); 
end 


* hl 
h1 = trimf(x(1),[e(2),e(3),e(4)]); % Rule 1:x1 is to zero 


% h2, Rule 2: xl is about +- pi/2,but smaller 
if x(1)<=0 
h2 = trinf(x(1),[e(2),e(2),e(3)]); 
else 
h2 = trinf(x(1),[e(3),e(4),e(4)]); 
end 


* h3, Rule 3: xl is about +- pi/2,but bigger 
if x(1)« 0 
h3 = trinf(x(1),[e(1),e(2),e(2) ); 
else 
h3 = trinf(x(1),[e(4),e(4), (5) ); 
end 


% h4, Rule 4: xl is about +- pi 
if x(1)«0 
h4 = trinf(x(1),[e(1),e(1),e(2)]); 
else 
h4 = trinf(x(1),[e(4),e(5),e(5)]); 
end 
hl + h2 + h3 + h4; 
ut = (h1 * ut1 + h2 * ut2 + h3 * ut3 + h4 * ut4)/(h1 + h2 + h3 + h4); 
sys(1) =ut; 


(5) 作 图 程序 : chap4_8plot. m, 


close all; 


figure(1); 
plot(t,x( :,1), 'r', t,x(:,2), 'b'); 
xlabel('time(s)'); ylabel( 'angle and angle speed response'); 
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figure(2); 
plot(t,ut(:,1), 'r'); 
xlabel('time(s)');ylabel('control input'); 


附录 新 的 LMI 求解 工具 荐 一 一 YALMIP I R $8 


线性 矩阵 不 等 式 (Linear Matrix Inequality,LMI) 是 控制 领域 的 一 个 强 有 力 的 设计 工 
具 。 许 多 控制 理论 及 分 析 与 综合 问题 都 可 简化 为 相应 的 LMI 问题 ,通过 构造 有 效 的 计算 机 
算法 求解 。 

随 着 控制 技术 的 迅速 发 展 ,在 反馈 控制 系统 的 设计 中 , 常 需要 考虑 许多 系统 的 约束 条 
件 , 例 如 ,系统 的 不 确定 性 约束 等 。 在 处 理 系统 鲁 棒 控 制 问题 以 及 其 他 控制 理论 引起 的 许多 
控制 问题 时 ,都 可 将 所 控制 问题 转化 为 一 个 线性 矩阵 不 等 式 或 带 有 线性 矩阵 不 等 式 约 束 的 
最 优化 问题 。 目 前 线性 矩阵 不 等 式 (LMIJ) 技 术 已 成 为 控制 工程 .系统 辨识 结构 设计 等 领域 
的 有 效 工 具 。 利 用 线性 矩阵 不 等 式 技术 来 求解 一 些 控制 问题 ,是 目前 和 今后 控制 理论 发 展 
的 一 个 重要 方向 。 

YALMIP 是 MATLAB 的 一 个 独立 的 工具 箱 , 具 有 很 强 的 优化 求解 能 力 , 该 工具 箱 具 
有 以 下 3 Ad ET. 

(D 是 基于 符号 运算 工具 箱 编写 的 工具 箱 。 

(2) 是 一 种 定义 和 求解 高 级 优化 问题 的 模 化 语言 。 

(3) 该 工具 箱 用 于 求解 线性 规划 、 整 数 规划 、 非 线性 规划 、 混 合 规划 等 标准 优化 问题 以 
及 LMI 问题 。 

采用 YALMIP 工具 箱 求 解 LMI 问题 ,通过 set 指令 可 以 很 容易 描述 LMI 约束 条 件 ,不 
需 具体 说 明 不 等 式 中 各 项 的 位 置 和 内 容 , 运 行 的 结果 可 以 用 double 语句 查看 。 

使 用 工具 箱 中 的 集成 命令 ,只 需 直 接 写 出 不 等 式 的 表达 式 , 就 可 很 容易 地 求解 不 等 式 
了 。YALMIP 工具 箱 的 关键 集成 命令 有 以 下 4 种: 

(1) 实 型 变量 sdpvar 是 YALMIP 的 一 种 核心 对 象 , 它 所 代表 的 是 优化 问题 中 的 实 型 
决策 变量 。 

(2) 约束 条 件 set 是 YALMIP 的 另外 一 种 关键 对 象 , 用 它 来 党 括 优化 问题 的 所 有 约束 
条 件 。 

(3) 求解 函数 solvesdp 用 来 求解 优化 问题 。 

(4) 求解 未 知 量 x 完成 后 ,用 x 一 double(x) 提 取 解 和 矩阵。 

YALMIP 工具 箱 可 从 网 络 上 免费 下 载 ,工具 箱 名 称 为 yalmip. rar。 工 具 箱 安装 方法 : 
(DAE Je rar 文件 解压 到 MATLAB 安装 目录 下 的 Toolbox 子 文件 夫 ; 加 然后 在 MATLAB 
界面 下 File->set path 单 击 add with subfolders, 然 后 找到 解压 文件 目录 。 这 样 MATLAB 
就 能 自动 找到 工具 箱 里 的 命令 了 。 

例如 ,求解 下 列 LMI 问题 LMI 不 等 式 为 

A'P -F'B'P -- PA + PBF < 0 


” 见 东北 大 学 王 琪 撰写 的 ¢YALMIP 工具 箱 简介 》。 


Co 4E A B P, REEF, 
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具体 的 一 个 求解 实例 如 下 : 
取 
[ —2.548 0 
A=| 1 =s] 0 
l0 —342 4 
| 0 0 
B—|0 1 0 
lo o 1 
[1000000 0 0 
P — |o 1000000 0 
LO 0 1000000 
解 该 LMI A ,可 得 
— 492.4768 一 5. 0 
F — | 一 5.05 一 494.0248 6.6 
0 6.6 一 495.0248 


仿真 程序 : chap4 9. m, 


clear all; 


close all; 


% First example 

B-[-2.5489.1 0;1 —11:;0 14.2 0]: 
B-[100;010;00 1]; 

F= sdpvar(3,3); 

P= 1000000 * eye(3) ; 


FAI-(A'tF'*B'Ü)*P*P*(A-*B*F); 
% LMI description 
L= set(FAI <0); 


solvesdp(L); 
F = double(F) 
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CHAPTER 5 


5.1 迹 代 学 习 控 制 的 数学 基础 


迭代 学 习 控 制 是 通过 迭代 修正 达到 某 种 控制 目标 的 改善 , 它 的 算法 较为 简单 , 且 能 在 给 
定 的 时 间 范 围 内 实现 未 知 对 象 实际 运行 轨迹 以 高 精度 跟踪 给 定期 望 轨迹 , 且 不 依赖 系统 的 
精确 数学 模型 ,因而 一 经 推出 ,就 在 控制 领域 得 到 了 广泛 的 运用 。 下 面 介 绍 学 习 控 制 的 稳定 
性 和 收敛 性 分 析 时 用 到 的 基本 数学 知识 ”” 。 


5.1.1 和 矩阵 的 迹 及 初等 性 质 
定义 RA 是 nn 阶 方 阵 , 则 称 A 的 主 对 角 元 素 的 和 为 A 的 迹 , 记 作 tr(4A)。 即 若 


ai QI2 Ain 
a2, A "'* A 
A = (5.1) 
i a i» a a 
[an á a, al 
则 
tr(A) = Pa; (5.2) 


ii 
WE A.B 都 是 n 阶 方 阵 ,A x TERR. UAR PERS a BOE IP TES: 
(1) tr(à Ao-p B) —AtrCA) -ptr(CB), 

(2) tr(A) — tr(A'), 

(3 4d A€CC""",BCC""",W| trCAB) —tr(BA), 


5.1.2. 向 量 范 数 和 和 矩阵 范 数 


1. 向 量 范 数 
[ER x€C",Hx-( & … &,)", 可 定义 


lx la = A lé | (5.3) 
i=l 
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lxl,= (D0) (5.4) 
lx d 一 max |£; | (5. 5) 
上 述 三 个 范 数 都 是 C" 中 的 向 量 范 数 ,分 别称 为 LCS 2-4 CRT oo BC C ECC 
上 都 是 p - 范 数 的 特殊 情形 。 
p ANGE LT 
Izl,-(Ml&l*) > 1&0 (5.6) 


2. 和 矩阵 范 数 
EX: 若 对 任意 矩阵 4EC”"… ,都 有 实数 eA | 与 之 对 应 , 且 满 足下 面 的 范 数 公理 
(1) 正定 性 : |A | 20,4 HC A=0 r [LA [| —0. 
(2) 齐 次 性 : 对 任何 MEC, laal =la lA Tl. 
(3) 三 角 不 等 式 : 对 任何 4,BEC” 8 
|I A+B l| ll A || 4- IB || 
则 称 这 个 实数 A | 为 矩阵 4 的 范 数 。 


lm ey |) (5. 7) 
j=l d=] 
lA ly, Smax|a;| ( 奥 比 雪夫 范 数 ) (5. 8) 
isj 
m n 1/p 
lalv (XZ lasl”) >, 1p <H (5. 9) 
—] k= 
lef e NE Hd 
* p—2M.K IAS LASS 75] Jal’) 为 4 ff Frobenius 范 数 ,简称 F- 范 
j=1 i-1 


数 , 是 最 常用 的 范 数 之 一 。 它 就 是 酉 矩阵 C"* 中 的 内 积 A41B 二 tr(B"MA ) 所 诱导 的 范 数 ， 
l4 l= | A) =rB A) — 9 M. [a | (5. 10) 


F- 范 数 具 有 下 列 良 好 的 性 质 : 
性 质 1 g ACC”, JAE UEC” VEC" BEA 


lA l= | UA || = IAV || p = || UAV || e (5, 11) 
性 质 2 RAEC, BEC"”, WE 
lAle< lAl lB Ilp (5.12) 


性 质 3 在 矩阵 空间 C "上 的 任意 实 函 数 , 记 为 外 . |, 如果 对 所 有 的 4,BEC" 一 ， 
AEC ,都 满足 以 下 4 个 条 件 : 

(D [Al 20,4 HE14 A—08E S8 [A | —0. 

(2 |aAl-la|llAI. 

(3) lA-—-BIsxIAI-IBI. 

(4) lABI «llAT IBI. 
则 称 || « I 为 相 容 的 矩阵 范 数 , 或 简称 矩阵 范 数 。 显 然 , 和 矩阵 的 F- 范 数 是 一 种 相 容 的 矩阵 
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5.2 迁 代 学 习 控 制 方 法 介绍 


迭代 学 习 控 制 (iterative learning control,ILC) 是 智能 控制 中 具有 严格 数学 描述 的 一 个 
分 支 。1984 年 ,Arimoto 等 提出 了 和 迭代 学 习 控 制 的 概念 ,该 控制 方法 适合 于 具有 重复 运动 
性 质 的 被 控 对 象 , 它 不 依赖 于 系统 的 精确 数学 模型 ,能 以 非常 简单 的 方式 处 理 不 确定 度 相 当 
高 的 非 线性 强 耦 合 动态 系统 。 目 前 ,迭代 学 习 控制 在 学 习 算 法 .收敛 性 .和 鲁 棒 性 .学 习 速 度 及 
工程 应 用 研究 上 取得 了 巨大 的 进展 。 

近年 来 , 友 代 学 习 控 制 理论 和 应 用 在 国外 得 到 快速 发 展 ,取得 了 许多 成 果 。 在 国内 ,和 迭 
代 学 习 控 制 理论 也 得 到 广泛 的 重视 ,有 许多 重要 著作 出 版 ” ,发 表 了 许多 综述 性 


论文 9。 
5.2.1 和 迭代 学 习 控制 基本 原理 


设 被 控 对 象 的 动态 过 程 为 
x(Q)-—-fGouG).D. yG)-—g(cQGO,uG)st) (5. 13) 
其 中 ,xER"、yER”、uER" 分别 为 系统 的 状态 、 输 出 和 输入 变量 ,f(*)、g(*) 为 适当 维 数 
的 向 量 函 数 , 其 结构 与 参数 均 未 知 。 寿 期 望 控制 ua(z) 存 在 , 则 迭代 学 习 控 制 的 目标 为 : 给 
定期 望 输出 y GO 和 每 次 运行 的 初始 状态 x; CO ,要 求 在 给 定 的 时 间 上 EL0,T 内 ,按照 一 定 的 
学 习 控 制 算 法 通过 多 次 重复 的 运行 ,使 控制 输入 ui C us CO ,而 系统 输出 y, G7 
yat), B k 次 运行 时 , 式 (5. 13) 表示 为 
Xi = Fr, yt (5. 14) 
跟踪 误差 为 
eilt) =y lt) — y, CQ) (5. 15) 
迭代 学 习 控 制 可 分 为 以 下 开 环 学 习 和 闭环 学 习 两 种 方法 : 
d) 开 环 学 习 控 制 的 方法 是 : 第 十 1 次 的 控制 等 于 第 & 次 控制 再 加 上 第 &A 次 输出 误 
差 的 校正 项 , 即 


Wt) —LGg G) se G2) (5. 16) 
(2) 闭环 学 习 控 制 的 方法 是 : 取 第 上 十 1 次 运行 的 误差 作为 学 习 的 修正 项 , 即 
Urn (t) — LG, Ct) 0,44 02) (85.17) 


其 中 ,L 为 线性 或 非 线 性 算 子 。 
5.2.2 基本 的 迭代 学 习 控 制 算法 


Arimoto 等 首先 给 出 了 线性 时 变 连续 系统 的 D Ak (eoe o) dil 
u,44 0) =u a) Té, Q) (5. 18) 
HB. 为 常数 增益 矩阵 。 在 D 型 算法 的 基础 上 ,相继 出 现 了 P 型 .PI 型 .PD 型 迭代 学 习 控 
制 律 。 从 一 般 意义 来 看 它们 都 是 PID 型 迭代 学 习 控 制 律 的 特殊 形式 ,PID 迭代 学 习 控 制 律 
表示 为 


ü, (D =u, 0) Hre, 0 He,t) 十 时 | e, (r )dr (5. 19) 
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其 中 , 卫 O 、 亚 为 学 习 增 益 和 矩阵 。 算 法 中 的 误差 信息 使 用 e, (1 ) 称 为 开 环 迭代 学 习 控制 ,如 
果 使 用 e, G) 则 称 为 闭环 迭代 学 习 控 制 , 如 果 同 时 使 用 e, (和 es GO B ERO F PH EI 2E 
代 学 习 控 制 。 

此 外 ,还 有 高 阶 迭 代 学 习 控制 算法 、 最 优 迭 代 学 习 控 制 算法 、 遗 忘 因 子 迭 代 学 习 控制 算 
法 和 反馈 -前 馈 迭 代 学 习 控 制 算法 等 。 


5.2.3 和 迭代 学 习 控 制 主要 分 析 方 法 


学 习 算 法 的 收敛 性 分 析 是 迭代 学 习 控 制 的 核心 问题 ,这 方面 的 研究 成 果 很 丰富 。 
1. 基本 的 收 剑 性 分 析 方法 


对 于 如 下 线性 离散 系统 : 
x(iG 4-1) —AxGO-- Bu(t) 
(5. 20) 
yG)-—Cx(G) 
迭代 学 习 控 制 算 法 为 
u, (t) =u, (t) +Te,t +1) (5.21) 


针对 学 习 算 法 式 (5.21) 的 收敛 性 ,有 以 下 两 种 分 析 方 法 : 
(1) 压缩 映射 方法 : 即 系统 要 求 满 足 全 局 Lipschitz 条 件 和 相同 的 初始 条 件 , 如 果 
|l I—CBT | 去 1, 则 有 
lern — || € — CBT )e, | < lIr—cBr || lle ll < Ile, ll (5. 22) 
此 时 算法 是 单调 收敛 的 。 该 方法 依赖 于 范 数 的 选择 ,常用 的 有 1 - 范 数 2:- 范 数 .2.- 范 数 及 
4- 范 数 。 在 收敛 性 证 明 过 程 中 常用 到 Bellman-Gronwall 引 理 。 
(2) 谱 半 径 条 件 法 : 如 果 谱 半径 o 满足 ol(I 一 CBT ) 二 0o 二 1, 则 有 
lim | e, | = lim | à — CBT e, |l = limp — CBP » lle, || (5. 23) 
B lim | e, || —0. 
2. 基于 2-D 理论 的 分 析 方 法 
迭代 学 习 控 制 系统 的 学 习 是 按 两 个 相互 独立 的 方向 进行 : 时 间 轴 方向 和 迭代 次 数 轴 方 
向 ,因此 和 迭代 学 习 过 程 本 质 上 是 二 维系 统 , 可 利用 成 熟 的 2-D 系统 理论 系统 地 研究 和 分 析 时 
间 域 的 稳定 性 和 和 迭代 次 数 域 的 收敛 性 问题 。2-D 系统 的 稳定 性 理论 为 迭代 学 习 控制 的 收 化 
性 证 明 提 供 了 一 种 非常 有 效 的 方法 ,2-D 系统 理论 中 的 Roesser 模型 成 为 迭代 学 习 控 制 中 


最 基本 的 分 析 模 型 。 
3. 基于 Lyapunov 直接 法 的 设计 方法 
Lyapunov 直接 法 已 广泛 用 于 非 线性 动态 系统 的 控制 器 设计 和 分 析 中 ,在 研究 非 线性 不 


确定 系统 时 ,该 方法 是 最 重要 的 应 用 工具 之 一 。 受 Lyapunov 直接 法 的 启发 ,在 时 间 域 和 选 
代 域 能 量 函 数 的 概念 得 到 研究 , 它 为 学 习 控 制 在 迭代 域 设计 和 收敛 性 分 析 方 面 提供 了 一 种 
新 的 研究 方法 。 

在 迭代 域 能 量 函 数 的 迭代 学 习 控 制 方法 基础 上 ,发 展 了 鲁 棒 和 自 适 应 迭代 学 习 控 制 , 可 
解决 具有 参数 或 非 参 数 不 确 定性 非 线性 系统 控制 器 的 设计 。 近 年 来 反映 时 间 域 和 和 迭代 域 系 
统 能 量 的 组 合 能 量 函 数 方法 也 应 用 于 过 代 学 习 控制 , 它 可 保证 在 迭代 域 跟踪 误差 的 渐进 收 
敛 以 及 在 时 间 域 具有 有 界 和 逐 点 跟踪 的 动态 特性 ,并 且 控 制 输入 在 整个 迭代 区 间 内 是 范 数 
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收敛 的 ,适用 于 一 类 不 具有 全 局 Lipschitz 条 件 的 非 线性 系统 。 通 过 能 量 函 数 的 方法 ,许多 
新 的 控制 方法 ,如 反 演 设计 和 非 线性 优化 方法 都 作为 系统 设计 工具 应 用 到 迭代 学 习 控 制 中 。 
此 外 ,还 有 最 优化 分 析 方 法 、 频 域 分 析 法 等 分 析 方 法 。 


5.2.4 和 迭代 学 习 控 制 的 关键 技术 


1. 学 习 算 法 的 稳定 性 和 收敛 性 

稳定 性 与 收敛 性 是 研究 当 学 习 律 与 被 控 系 统 满足 什么 条 件 时 ,迭代 学 习 控 制 过 程 才 是 
稳定 收敛 的 。 算 法 的 稳定 性 保证 了 随 着 学 习 次 数 的 增加 ,控制 系统 不 发 散 , 但 是 对 于 学 习 控 
制 系统 而 言 ,仅仅 稳定 是 没有 实际 意义 的 ,只 有 使 学 习 过 程 收 敛 到 真 值 ,才能 保证 得 到 的 
控制 为 某 种 意义 下 最 优 的 控制 。 收 敛 是 对 学 习 控 制 的 最 基本 要 求 , 多 数学 者 在 提出 新 的 
学 习 律 的 同时 ,基于 被 控 对 象 的 一 些 假设 ,给 出 了 收敛 的 条 件 。 例 如 ,Arimoto 在 最 初 提 
出 PID 型 学 习 控 制 律 时 , 仅 针 对 线性 系统 在 D 型 学 习 律 下 的 稳定 性 和 收敛 条 件 作 了 
证 明 。 

2. 初始 值 问题 

运用 迭代 学 习 控 制 技 术 设 计 控 制 器 时 ,只 需要 通过 重复 操作 获得 的 受 控 对 象 的 误差 或 
误差 导数 信号 。 在 这 种 控制 技术 中 ,和 迭代 学 习 总 要 从 某 初 始点 开始 ,初始 点 指 初始 状态 或 初 
始 输出 。 几 乎 所 有 的 收敛 性 证 明 都 要 求 初 始 条 件 是 相同 的 ,解决 迭代 学 习 控 制 理论 中 的 初始 
条 件 问 题 一 直 是 人 们 追求 的 目标 之 一 。 目 前 已 提出 的 迭代 学 习 控 制 算法 大 多 数 要 求 被 控 系 统 
每 次 运行 时 的 初始 状态 在 期 望 轨迹 对 应 的 初始 状态 上 , 即 满足 初始 条 件 : 

z,(0) 9x40), &—0,1,2,- (5. 24) 

当 系 统 的 初始 状态 不 在 期 望 轨迹 上 ,而 在 期 望 轨迹 的 某 一 很 小 的 邻 域内 时 ,通常 把 这 类 
问题 归结 为 学 习 控 制 的 鲁 棒 性 问题 研究 。 

3. 学 习 速 度 问题 

在 迭代 学 习 算 法 研究 中 ,其 收敛 条 件 基本 上 都 是 在 学 习 次 数 & 一 ce 下 给 出 的 。 而 在 实 
际 应 用 场合 ,学 习 次 数 & 一 吕 显 然 是 没有 任何 实际 意义 的 。 因 此 ,如 何 使 迭代 学 习 过 程 更 快 
地 收敛 于 期 望 值 是 迭代 学 习 控 制 研究 中 的 男 一 个 重要 问题 。 

ILC 本 质 上 是 一 种 前 馈 控制 技术 ,大 部 分 学 习 律 尽管 证 明了 学 习 收 敛 的 充分 条 件 ,但 收 
敛 速度 还 是 很 慢 。 可 利用 多 次 学 习 过 程 中 得 到 的 知识 来 改进 后 续 学 习 过 程 的 速度 ,例如 , 采 
用 高 阶 迭 代 控 制 算 法 、 带 遗忘 因子 的 学 习 律 .利用 当前 项 或 反馈 配置 等 方法 构造 学 习 律 ,可 
使 收敛 速度 大 大 加 快 。 

4. 鲁 棒 性 问题 

迭代 学 习 控 制 理论 的 提出 有 浓厚 的 工程 背景 ,因此 仅仅 在 无 干扰 条 件 下 讨论 收敛 性 问 
题 是 不 够 的 ,还 应 讨论 存在 各 种 干扰 的 情形 下 系统 的 跟踪 性 能 。 一 个 实际 运行 的 迭代 学 习 
控制 系统 除了 存在 初始 偏 移 外 ,还 或 多 或 少 存在 状态 扰动 ,测量 噪声 、 输 入 扰动 等 各 种 干扰 。 
鲁 棱 性 问题 讨论 存在 各 种 干扰 时 迭代 学 习 控 制 系统 的 跟踪 性 能 。 具 体 地 说 ,一 个 迭代 学 习 
控制 系统 是 鲁 棒 的 , 指 系统 在 各 种 有 界 干 扰 的 影响 下 ,其 迭代 轨迹 能 收敛 到 期 望 轨迹 的 邻 域 
内 ,而 当 这 些 干扰 消除 时 ,迭代 轨迹 会 收敛 到 期 望 轨迹 。 
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5.3 ”机械手 轨迹 跟踪 和 运 代 学 习 控 制 仿真 实例 


5.3.1 控制 器 的 设计 


考虑 一 个 n 关节 的 机 械 手 ,其 动态 性 能 可 以 由 以 下 二 阶 非 线性 微分 方程 描述 : 
M(q)0q —- C(q.q0d + G(g) —u — t, (5. 25) 

HP, ER 为 关节 角 位 移 量 ,M(q)ER" HIW F WREE, Cd) ER 表示 离心 
力 和 哥 氏 力 ,G(qg)ER" 为 重力 项 ,r € R" 为 控制 力矩 ,rs€R" 为 各 种 误差 和 扰动 。 

设 系 统 所 要 跟踪 的 期 望 轨迹 为 y(t),t:E[0,T]。 系 统 第 i 次 输出 为 y; a) Se (1)== 
YE 

在 学 习 开始 时 ,系统 的 初始 状态 为 x,(0)。 学 习 控 制 的 任务 为 通过 学 习 控 制 律 设计 
ui+1(t) ,使 第 i 十 1 次 运动 误差 e; ;1(1) 减 少 。 采 用 以 下 三 种 基于 反馈 的 迭代 学 习 控 制 律 : 

(OD 闭环 了 D 型: 


upy (E) =u OG) Rdtt) 944402) (5. 26) 
(2) 闭环 PD 型 
u,4 0) =u, G) +K, qa) —q14 00 H Kalat) 74,4 00) (5. 27) 
(3) 指数 变 增益 D 型 ; 
Un 00 =u, (t) +K, (40) —q,4 00 + Ki (Ga(t) 7414 00) (5. 28) 


5.3.2 仿真 实例 


本 节 针 对 二 关节 机 械 手 ,介绍 一 种 机 械 手 PD 型 反馈 迭代 学 习 控 制 的 仿真 设计 方法 。 
针对 二 关节 机 械 手 控制 系统 式 (5. 250 ,各 项 表示 为 : 
M = Lmg l 
RB uma mV Hm LS Ll acosqga) +I +H I5 mg = ma =m? + Lila cosg ) + 
l4 mg mU, -T,, 
C= Lcs hi 
其 中 ,ci 二 hg syc1s 二 hg i 十 hq sca =— hq 15.05 0. h =— ml; lasing: 
G-—[G, G,]' 
其 中 ,Gj 二 (ml 十 mzli)gcosqi 十 mzlizgcos(qi1 十 qs),G, 二 mslizgcos(gi 十 gs), 干 扰 项 为 
ts=[0. 3sint 0.1(1—e™)]". 
机 械 手 系统 参数 为 mi! =10,m;, =5,l 1.45 一 0.5, /一 0.5, 1 一 0.25,7 =0. 83, 
1,0.3,g —9.8. 
根据 式 (5. 26) — 3X C5. 280 ,采用 三 种 闭环 迭代 学 习 控 制 律 ,其 中 ,M=1 为 D 型 迭代 学 
习 控 制 ,M 二 2 为 PD 型 迭代 学 习 控 制 ,M 二 3 为 指数 变 增益 D 型 迭代 学 习 控制 。 
两 个 关节 的 角度 指令 分 别 为 sin(3t) 和 cos(3t) ,为 了 保证 被 控 对 象 初始 输出 与 指令 初 
值 一 致 , 取 被 控 对 象 的 初始 状态 为 x(0)=[0 3 1 0]'。 取 PD 型 迭代 学 习 控 制 , 即 
M= 王 2 ,仿真 结果 如 图 5-1 一 图 5-3 BER 
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图 5-2 第 20 次 迭代 学 习 的 角度 跟踪 
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change of maximum absolute value of errorl and error2 with times i 


angle tracking error of lmil and link 2 


10 
time(s) 


change of maximum absolute value of derrorl and derror2 with times i 


angle speed tracking error of imil and link 2 


time(s) 


5-3 第 20 次 迭代 学 习 角 度 和 角速度 跟踪 误差 收敛 过 程 


仿真 程序 如 下 : 
COD 主 程序 ; chap5 lmain. m. 


clear all; 
close all; 


t-2[0:0.01:3]'; 

k(1:301) = 0; * Total initial points 
k-7k'; 

T1(1:301) = 0; 

T] = T1'; 

T2 = T1; 

T-[T1 72]; 
LELLLLLLELLLLILLELLELL LEE LEELLELEELEL LE 
M- 20; 

for i-0:1:M % Start Learning Control 
i 


pause(0.01); 


sim('chap5 1sim',[0,3]); 
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q1239(:,1); 
dql = q(:,2); 
q2-73(:,3); 
dq2 - q(:,4); 


qld= qd(:,1); 
dald- qd(:,2); 
q2d - qd(:,3); 
dq2d= qd( :,4); 


el-7gld- ql; 
e2 = q2d - q2; 
del = dqld- dql; 
de2 = dq2d - dq2; 


figure(1); 

subplot(211); 

hold on; 

plot(t,qi, 'b', t, qid, 'x*); 
xlabel('time(s)');ylabel('qld,q1 (rad)'); 


subplot(212); 

hold on; 

plot(t,q2, 'b',t,q2d, 'r*); 
xlabel('time(s)');ylabel('q2d,q2 (rad)'); 


j*-i*1; 

times(j) = i; 

eli(j) = max(abs(el)); 
e2i(j) = nax(abs(e2)); 
deli(j) = max(abs(del)); 
de2i(j) » nax(abs(de2)); 


end * End of i 
EELLLLLILLLLLLLELELLILELLILLLILLLLILLLLLLLILI 
figure(2); 

subplot(211); 


plot(t, ld, 'r',t,q1, 'b'); 

xlabel( 'time(s)');ylabel('angle tracking of link 1'); 
subplot(212); 

plot(t, q2d, 'r',£,q2, 'b'); 
xlabel('time(s)');ylabel('angle tracking of link 2"); 


figure(3); 

subplot(211); 

plot(t,dqld, 'r',t,dql, 'b'); 

xlabel('time(s)'); ylabel('angle speed tracking of link 1'); 
subplot(212); 

plot(t,dq2d, 'r',t,dq2, 'b'); 
xlabel('time(s)');ylabel('angle speed tracking of link 2"); 


figure(4); 
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subplot(211); 

plot(times,eli,'* - r',times,e2i, 'o— b'); 

title('change of maximum absolute value of errorl and error2 with times i'); 
xlabel('time(s)');ylabel('angle tracking error of lmil and link 2"); 
subplot(212); : 

plot(times,deli,'* - r',times,de2i, 'o- b'); 

title('change of maximum absolute value of derrorl and derror2 with times i'); 
xlabel('time(s)');ylabel('angle speed tracking error of lmil and link 2"); 


(2) Simulink 子 程序 : chap5 lsim. mdl, 


To Wotkspace1 


To Workspace? 
chap5 1plant 
To Ww ce2 
S-Function odotaa 
S-Function1 


From 
Wodspacet 


Constant 


Clock To Workspace 


(3) 被 控 对 象 子 程序 : chap5 lplant. m, 


function [sys, x0, str,ts] = spacemodel(t, x, u, flag) 
switch flag, 
case 0, 
[sys, x0, str, ts] = ndlInitializeSizes; 
case 1l, 
sys = ndlDerivatives(t,x,u); 
case 3, 
sys = mdlOutputs(t,x,u); 
case (2,4,9) 
sys-[]; 
otherwise 
error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0, str, ts] = mdlInitializeSizes 


Sizes - simsizes; 


I 


sizes.NumContStates 
sizes.NumDiscStates  - 


4 
0 
sizes.NumOutputs = 4; 
sizes. NumInputs = 2 
1 


sizes.DirFeedthrough = 
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sizes.NumSampleTimes = 1; 

SyS = simsizes(sizes); 

x0 = [0;3;1;0]; * Must be equal to x(0) of ideal input 
str e [] 

ts - [00]; 


function sys = mdlDerivatives(t, x,u) 
Tol- [u(1) u(2)]'; 


g79.8; 
ml=10;m2=5; 
1121;12720.5; 
10172 0.5;1c27 0.25; 
1120.83;122 0.3; 


dil-2mli*1cl1^24m2* (11^2* 1c2^2* 2* 11 * 102 * cos(x(3))) + I1 + I2; 
d12 = m2 * (1c2^2 + 11 * 1c2 * cos(x(3))) + I2; 

d21 = d12; 

d22 7 m2 * 1c2^2 * I2; 

D- [dll d12;d21 d22]; 

h= -m2 * 11 * 1c2 * sin(x(3)); 

cli-h*x(4); 

cl2-h*x(4) t h* x(2); 

c21= -h* x(2); 

c22-70; 

C= [etl 612;c21 c22]; 

gl = (ml * lcl + m2 * 11) * g* cos(x(1)) * m2* 1c2*g * cos(x(1) * x(3)); 
g2 = m2 * lc2 * g * cos(x(1) * x(3)); 

G= [gl;g2]; 


a=1.0; 

dl-a*0.3* sin(t); 
d2-a*0.1*(1-exp(-t)); 
Td = [d1;d2]; 


S= — inv(D) *C* [x(2);x(4)] - inv(D) * G+ inv(D) * (Tol- Td); 


sys(1) = x(2); 
sys(2) = S(1); 
sys(3) = x(4); 
sys(4) = S(2); 
function sys = mdlOutputs(t, x,u) 


sys(1) = x(1); € Anglel:q1 
sys(2) = x(2); * Anglel speed:dql 
sys(3) = x(3); € Angle2:q2 
sys(4) = x(4); * Angle2 speed:dq2 


(D 控制 器 子 程序 : chap5_1lctrl. m. 


function [sys,x0, str,ts] = spacemodel(t, x, u, flag) 
switch flag, 
case 0, 
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[sys, x0, str, ts] = ndlInitializeSizes; 
case 3, 
sys = ndlOutputs(t, x,u); 
case (2,4,9) 
sys- []; 
otherwise 
error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0, str, ts] = mdlInitializeSizes 


sizes - simsizes; 


sizes.NumContStates 
sizes.NumDiscStates  - 


sizes.NumInputs = 
sizes.DirFeedthrough = 


0 
0 
sizes.NumOutputs = 2; 
8 
1 
1 


sizes.NumSampleTimes 


Sys 7 simsizes(sizes); 


x0 = [Eh 
str = []; 
ts - [00]; 


function sys = mdlOutputs(t, x, u) 
qld=u(1);dqld= u(2); 
q2d = u(3);dq2d = u(4); 


ql - u(5);dql = u(6); 
q2 = u(7) ;dq2 = u(8); 


el=qld- ql; 
e2 = q2d - q2; 
e= [el e2]'; 
del = dqid - dqi; 
de2 = dq2d - dq2; 
de = [del de2]'; 


Kp= [100 0;0 100]; 
Kd - [500 0;0 500]; 


M= 2; 
if M== 
Tol = Kd * de; % D Type 
elseif M-- 
Tol = Kp* e + Kd * de; % PD Type 
elseif M-- 
Tol = Kd * exp(0.8 * t) * de; % Exponential Gain D Type 
end 


sys(1) » Tol(1); 
sys(2) » To1(2); 


(5) 指令 程序 : chap5. linput, m, 


function [sys,xO,str,ts] = spacemodel(t, x, u, flag) 
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switch flag, 
case 0, 

[sys, x0, str, ts] = ndlInitializeSizes; 
case 3, 

sys = mdlOutputs(t, x,u); 
case (2,4,9) 

sys- []; 
otherwise 

error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0, str, ts] = mdlInitializeSizes 
sizes - simsizes; 
sizes. NumContStates = 
sizes. NumDiscStates = 
sizes. NumOutputs = 
sizes. NumInputs = 
sizes. DirFeedthrough = 
sizes. NumSampleTimes = 


erereOoOoOoRAROO 
`~ 


sys = simsizes(sizes); 


x0 - []; 
str [D]; 
ts - [00]; 


function sys = mdlOutputs(t,x, u) 
qld= sin(3* t); 

dqld = 3 * cos(3 * t); 

q2d = cos(3 * t); 

dq2d= -3 * sin(3* t); 


sys(1) = qid; 
sys(2) = dqld; 
sys(3) = q2d; 
sys(4) = dq2d; 


5.4 线性 时 变 连 续 系 统 迭 代 学 习 控 制 


5.4.1 系统 描述 


Arimoto ™ 等 给 出 了 线性 时 变 连续 系统 为 


x(t) =A(t)x(t)+B(t)u(t) 
(5. 29) 


yG) —-CGxG) 
其 开 环 PID 型 迭代 学 习 控制 律 为 


aO m O + (P Le Jar)eco (5. 30) 
其 中 ,TT 江 , 更 为 学 习 增 益 和 矩阵 。 
5.4.2 控制 器 设计 及 收敛 性 分 析 
5EXB 5.1/7 车 由 式 (5. 29) 和 式 (5. 30) 描 述 的 系统 满足 如 下 条 件 : 
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(D | 1—CCGOBCGOT CO | X1. 

(2) 每 次 近代 初始 条 件 一 致 , 即 x, (00 9x, (k — 1.2.3.2. y, (OO — y, CO) LB] 34 c 
时 ,有 ye GCOy4 OD. Vr€ [0. T ]. 

下 面 给 出 该 定理 的 简单 分 析 ,可 参考 文献 [4 的 证 明 过 程 。 

由 式 (5. 29) 及 条 件 式 (5. 30) 得 

gin (00 5 Cx444€00 Cx, (0) — y, (0) 

ll] e, (0)= 二 0(k 二 0,1,2,…), 即 系统 满足 初始 条 件 。 

非 齐 次 一 阶 线性 微分 方程 xX G0 —A GOx GO) HB GOu GO RS ft 29 


skt) =Cexp(| Aar) 二 exp(| Ade) | Bou exp, [^ —Adà d 
—Cexp(At) | expCAt )| BcoucoexpC- Ade 


— Cexp(Ar) +f expCA G —rt))B(r)u(r)dr 
WO (t.c) —exp(A G — 22) . M 
X0 — XXL40) -f'o (FE BL CT) — tra Cr) Jdr 


由 于 e, G) — ya G2 — y, Go sery OD) y O)— yry OD Dn 
Ery lt) — e, C) = y, 0) — y 44 0) —CGODQG 0) — x44 O0 


- [ew Ø G ,c)BGOGQ, G) — wn G))de 
即 
PN, ETA S, - [ewe riB] —u,(cY)dr 
将 PID 型 控制 律 式 (5. 30) 代 入 上 式 , 则 第 & 十 1 次 输出 的 误差 为 : 
"ROT - [eo 6 G,OBGLP Gà, G 4 LGe G 4-9 C| e, C3) dé ^de 


(5.31) 
fI FH ^r SB ASXA S GG. —CGOBGOD GO. 


[l'ecosco Fi, Ge Str ir) 


-[ T OG x je Cidde 
0 0 dr 


| g 
-cOBG) P Ges Gc) — | 3,6 0 «2e, Ode (5. 32) 
4o OT 
将 式 (5. 32) 代 入 式 (5. 3D ,得 


t | 
ea 0) —[I — CGOBGOT G)]e, G) «f. 3,9 0 e COdr 一 
| co ee Bs a 


| | ew aoa g Ce; dødt (5. 33) 


04 0 


TEX Co. 33) 两 端 取 范 数 , 有 
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le; CO || de + 


T 9 
en O ll x I 1—cO»BGO P €) ll «eC I +| [eu 


| ley dit opema i beliet 


rr eco © (tT Br) 9 GO || lle; Co) || dedz 


< |1—cCGoBGOP €) | lle, C) ll [5 le; Cc) || dr + 


NIC ll e, Co) || dade (5. 34) 
其 中 ， 
b, —max| sup. GG.) NU lC G) 6 G.OBGOLCGO | 
b.— sup. Il CCo 6 G.BGo 9 CO Il 
将 式 (5. 34) 两 端 同 乘 以 expC —A0 ,4 之 0, 并 考虑 | expQr) dr 一 < 一 ,有 


expa Df b, le, CO || dr Sepan f b, l e, CO || exp(—àr)exp(àr)dr 


< b,expC— àt) l| e, CO Il ,| sxpwr)dr 


exp(At)—1 


—b,exp(C— A2 lle, CO ll, 1 


b 
3 le, CO ll ,exp( 一 Mt)CexpGQz) — D 


(1— exp(— A10) 


=b, A le; CO Il 
« y, SERA peo pl, 
根据 范 数 的 性 质 中 ,有 
expan | 6, leita Il dadz a [LEE le, CO | 
则 
lei la SA lle lla (5. 35) 
aspa ROAD a, (CAD), AF oci A 取 足 够 大 时 ,可 


使 p. 因此 lim | €, ll 170, 


如 果 将 式 (5. 30) 中 的 e(k) 改 为 e(k 十 1), 则 为 闭环 PID 型 迭代 学 习 控 制 律 。 同 定理 5.1 
的 分 析 过 程 ,可 证 明 闭 环 PID 迭代 学 习 控 制 律 。 


5.4.3 仿真 实例 
考虑 二 输入 、 二 输出 线性 系统 : 
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pd pe a sl 
= 二 
rij) 1 l| [r;CO O l| jua) 
E n f i ESICA 
(1)| lo | 


期 望 跟踪 轨迹 为 
ME Ir t€ [0,1] 
za (2 icosC3t ) 
0. 95 


由 于 CB= : | QD = | 
0 1 0 


MILLE LEE 


iu. | ,系统 的 初始 状态 为 ped - H | 
0 2.0 meč) li 

在 chap5. 2sim. mdl 程序 中 ,选择 Simulink 的 Manual Switch 开关 ,将 开关 向 下 , 取 PD 
型 开 环 迭代 学 习 控 制 律 ,仿真 结果 如 图 5-4 一 图 5-6 所 示 。 将 开关 向 上 ,采用 PD 型 闭环 和 迭 
代 学 习 控 制 律 ,仿真 结果 如 图 5-7 一 图 5-9 所 示 。 可 见 ,闭环 收敛 速度 好 于 开 环 收敛 速度 。 


中 取 工 | 


xld,xl 


x2d,x2 


0 0.2 0.4 0.6 0.8 1 
time(s) 


5-4 30 次 迭代 学 习 的 跟踪 过 程 ( 开 环 PD 控制 ) 


1 x 
Full" 
0 


position tracking of x2 position tracking of x1 


0 0.2 0.4 0.6 0.8 1 
time(s) 
2 
1 
0. 
lg 0.2 0.4 0.6 0.8 l 
time(s) 


图 5-5 第 30 次 迭代 学 习 的 位 置 跟踪 ( 开 环 PD 控制 ) 
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xld,xl 


"0 0.2 0.4 0.6 0.8 1 
time(s) 


0 0.2 0.4 0.6 0.8 1 
time(s) 


5-6 30 次 迭代 学 习 的 跟踪 过 程 ( 闭 环 PD 控制 ) 


position tracking of x1 
eo 
A 


0 0.2 0.4 0.6 0.8 1 

time(s) 

Q 

s 2 

oD 

c 

xod 

£ 

s 0 

z 3 

& 0 0.2 0.4 0.6 0.8 1 
time(s) 


图 5-7 $8 30 次 迭代 学 习 的 位 置 跟踪 (闭环 PD 控制 ) 
change of maximum absolute value of errorl and error2 with times 
60 
50 
40 


30 


error] and error2 


25 30 


time(s) 
图 5-8 30 次 迭代 过 程 中 误差 最 大 绝对 值 的 收敛 过 程 ( 开 环 PD 控制 ) 
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change of maximum absolute value of errorl and error2 with times 


errorl and error2 


0 & 30 B 20 25 30 
time(s) 


5-9 30 次 迭代 过 程 中 误差 最 大 绝对 值 的 收敛 过 程 (闭环 PD 控制 ) 


仿真 程序 如 下 : 
(D 主 程序 ; chap5_2main. m, 


% Iterative D- Type Learning Control 
clear all; 


close all; 


t-[0:0.01:1]*'; 

k(1:101) = 0; * Total initial points 
k=k'; 

T1(1:101) = 0; 

T1-2T1'; 

T2 = Tl; 

T= [T1 T2]; 


k1(1:101) 70; % Total initial points 
ki =k1'; 

E1(1:101) = 0; 

El-E1'; 

E2 - E1; 

E3 = El; 

EA = El; 

E- [El E2 E3 E4]; 
LLLLLLLLLLLLLLLLLLLELLEELLLLLLLLELLLILILI 
M 7 30; 

fori-0:1:M % Start Learning Control 
1 


pause(0. 01); 


sim('chap5 2sim',[0,1]); 


xi2x(:1); 


x27 x(:,2); 


xld- xd(:,1); 
x2d = xd( :,2); 
dxld= xd(:,3); 
dx2d = xd(:,4); 


el-E(:,1); 
e2-E(:,2); 
del-E(:,3); 
de2-E(:,4); 
e= [el e2]'; 
de = [del de2]'; 


figure(1); 

subplot(211); 

hold on; 

plot(t,xl, 'b';t,xld, 'r'); 


xlabel('time(s)");ylabel('xid,x1'); 


subplot(212); 
hold on; 
plot(t,x2, 'b', t, x2d, 'r'); 


xlabel('time(s)');ylabel('x2d,x2'); 


j=i+1; 

times(j) = i; 

eli(j) = max(abs(e1)); 
e2i(j) = max(abs(e2)); 
deli(j) = max(abs(del)); 
de2i(j) = max(abs(de2)); 


end % End of i 
LEELLLLLLLLLLLLLLLLELLLLILLLLLLLLLELLLLLLEI 
figure(2); 

subplot(211); 


plot(t,x1d, 'r',t,xl, 'b'); 


xlabel('time(s)');ylabel('position tracking of x1'); 


subplot(212); 
plot(t,x2d, 'r',t,x2, 'b'); 


xlabel('time(s)');ylabel('position tracking of x2'); 


figure(3); 


plot(times,eli,'* — r',times,e2i, 'o— b'); 
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title('Change of maximum absolute value of errorl and error2 with times'); 


xlabel('time(s)'); ylabel('errorl and error2'); 
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(2) Simulink 程序 : chap5_2sim. mdl。 


To Workspacet 


chap5_2plant 
To Wokspace2 


chap5_2input S-Function 


S-Function1 


Manual Switch S-Function2 


Clock To Workspace 


Derivative 


(3) 被 控 对 象 子 程序 : chap5 2plant. m, 


function [sys,x0, str,ts] = spacemodel(t, x, u, flag) 
switch flag, 
case 0, 
[sys, x0, str, ts] = ndlInitializeSizes; 
case 1, 
sys = ndlDerivatives(t,x,u); 
case 3, 
sys = mdlOutputs(t,x,u); 
case {2,4,9} 
sys-[]; 
otherwise 
error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0, str, ts] = ndlInitializeSizes 


sizes = simsizes; 


LU 
= ot NO PP 


sizes.NumContStates 


LU 


sizes.NumDiscStates 
sizes.NumOutputs 


sizes.NumInputs 


sizes.DirFeedthrough 


sizes.NumSampleTimes 


SyS 7 simsizes(sizes); 


x0 = [0;1]; 
str = []; 
ts - [00]; 


function sys = mdlDerivatives(t,x,u) 

A-[-23;11]; 

C-[10;01]; 

B-[11;01]; 

Gama 7 0.95; 

norm(eye(2) - Cx B * Gama); * Must be smaller than 1.0 


U= [u(1);u(2)]; 
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dx-A*x*tB*U; 

sys(1) » dx(1); 

sys(2) = dx(2); 

function sys = mdlOutputs(t, x, u) 
sys(1) =x(1); 

sys(2) = x(2); 


(D 控制 器 子 程序 : chap5 2ctrl. m, 


function [sys, x0, str,ts] = spacemodel(t, x,u, flag) 
switch flag, 
case 0, 

[ sys, x0, str, ts] = mdlInitializeSizes; 
case 3, 

sys = ndlOutputs(t,x,u); 
case (2,4,9) 

sys-[]; 
otherwise 

error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0, str,ts] » mdlInitializeSizes 
sizes = simsizes; 


sizes.NumContStates 


sizes.NumDiscStates 


sizes.NumOutputs 


sizes.NumInputs 


sizes.DirFeedthrough 


LU 
ere ANOO 
T 


sizes.NumSampleTimes 


SyS 7 simsizes(sizes); 


xo. = [n 
str = []; 
ts - [00]; 


function sys = mdlOutputs(t, x, u) 
el-u(1);e2-7 u(2); 
del = u(3);de2 = u(4); 


e= [el e2]'; 
de = [del de2]'; 


Kp= 2.0; 
Gama = 0. 95; 
Kd = Gama * eye(2); 


Tol = Kp * e + Kd * de; % PD Type 


sys(1) = Tol(1); 
sys(2) = To1(2); í 


(5) 指令 程序 : chap5. 2input. m, 


function [sys,xO,str,ts] = spacemodel(t, x,u, flag) 
switch flag, 
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case 0, 

[sys, x0, str, ts] = mdlInitializeSizes; 
case 3, 

sys = mndlOutputs(t, x,u); 
case (2,4,9) 

sys-[]; 
otherwise 

error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0, str,ts] » ndlInitializeSizes 
Sizes - simsizes; 
sizes.NumContStates - 
sizes.NumDiscStates = 
sizes.NumOutputs z 
sizes.NumInputs = 
sizes.DirFeedthrough - 


PS o0 
LO 


sizes.NumSampleTimes = 


sys = simsizes(sizes); 


x0 - [] 
str = []; 
ts - [00]; 


function sys = mdlOutputs(t, x, u) 
xld= sin(3*t); 

dxld = 3 * cos(3* t); 
x2d = cos(3* t); 

dx2d= -3 * sin(3 * t); 


sys(1) = xid; 
sys(2) = x2d; 


sys(3) = dxld; 
sys(4) = dx2d; 


5.5 任意 初始 状态 下 的 迭代 学 习 控 制 


下 面 介绍 一 种 任意 初始 状态 下 的 学 习 控 制 方法 "" 及 其 仿真 设计 方法 。 


5.5.1 问题 的 提出 


假设 一 种 系统 为 
x G) —AxCGO-- Bu(t) 


y(t1)=Cx(t) (5. 36) 
x(t,) —x(0) 
其 中 ,x (1) ER ,u(t),y(1)ER” ,A .B.C 为 相应 维 数 的 常 阵 且 满足 假设 
rank(CB) =m (5.97) 


设 系统 所 要 跟踪 的 期 望 轨迹 为 ya) ELOT] RRB i 次 输出 为 y; (1), 令 ea) 
Yai) =J G2, 
在 学 习 开 始 时 ,系统 的 初始 状态 为 xv(0) ,初始 控制 输入 为 uw,(t)。 学 习 控 制 的 任务 为 
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ERIS i UOS SIS u, G) x, CRI e; GO ,通过 学 习 控 制 律 设计 心心 (人 和 xi COO ,第 ;十 1 
次 运动 误差 e ,1,(1) 将 减少 。 


5.5.2 控制 器 的 设计 


首先 介绍 范 数 如 下 : 
| (5. 38) 
SiS m 
IG I. = max | > [gi |) (5. 39) 
Kim V 
Let» p, = sup lexpC—A20 ll eco |.) (5. 40) 
RP. 


HEP, e UA eM ER 中 的 第 i 个 元 素 ,g"7 是 GER”” 中 的 第 i,ji T2600, 
学 习 控 制 律 及 初始 状态 学 习 律 分 别 为 
u; (t) —u,G) + Lé,G) (5.41) 
x,4 (0 —x,(0) + BLe,(0) (5.42) 
其 中 ,LER”” 为 常 阵 。 
定理 5.2"” 若 学 习 控 制 律 (5. 41) 及 初始 状态 学 习 律 (5. 42) 满 足以 下 条 件 : 
(1) uo lt) 在 [0,T] 上 连续 ,y,(?) 在 [0,T] 上 连续 可 微 。 
(2) || L..—CBL | «1. 
则 当 ;— ofr A 
yy” yalt. ET] 
下 面 给 出 该 定理 的 详细 分 析 过 程 ,可 参考 文献 [10] 的 证 明 过 程 。 
由 方程 式 (5. 36) 的 解 为 
xG) =exp(ADx(0) + | exp A G — Bu Cde 
则 
x, 4) m expLA Dx, (0) + | exp(A G — Bin Ode 
将 式 (5.41) 和 (5.42) 代 和 人 上 式 , 得 
x, () — exp AD x, (0) + BLe, (0)] + ['expCA G — 0 LBu, (t) -- Lë, CO de 
则 
Gy a —CxiauC 


= y(t) = C| exptADLx, (0) + BLe, (0) ) +| expCA Ct — c)» [Bu, (t) tLe,(t) ]dc 
= yt) — |Cexp (Ad )x, (0) 4- CexpCADBLe, (0) 十 


| cexpta a = Bd 4 ['cexpa —g 58S, (eode. 
采用 分 部 积分 方法 : 


| rydt —ry li =| iydt 
0 0 
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则 


[ceau = rJIBLË td 
—[Cexp(A G —7))BLe, 0] |’ -| DCAexp(A (? — 2) BLe, (dc 
— CBLe, (t) — Cexp( At )B Le, (0) * ['CAexpta — BL dr 
e; 0) — y 4) — Cexp(CAnD x, (CO) — CexpCAD BLe; CO) - | cexpta u —rt))Bu;(t)dr 一 
CBLe, (1) + Cexp AD BLe, C0 - | cAexpA 4 — eJ)BLe, tv fde 
=ya(t) clear * exta a — Bu, COde - 


CBLe, (1) -| CAexplA tt —eYOBLe ydr 


= yQ4QG) — y; (t) — CBLe.(t) -| CAexp(A(t—7))BLe,(r)dr 
0 


即 
einlt) =e; 0) — CBLe, C) — | CAexp(AG — £)BLe; Ode 
= (In —CBL)e, (t) — | CAexp(AG — £)BLe, Gd 
Es LIEU e IU BG EIE 
Ixvlslixi lvi. üix--vlisix!i- lvi 
则 


expC— A2 ll e; (G2 |l. 


< | G, —CBL)e,G) || ;expC —A0 + |[ cavau 一 rz))BLe,(r)dr| exp(—41) 
< || (用 一 CBE) ||- || e; @) ll ;expC—A0 十 | CA4exp(4A( 一 rz))BLei(r)dr| expC— A0) 
= | Aa — CBL) | ..1e;G»? ll, || CABL ||- | exp(A(t—r))e,(r)dr| exp(—A0 
= |, — CBL) | . | e, G2 || + ll CABL || ll hGO ll, 


其 中 ,h(z)= | expCA (一 rz))ei(Cr)dr。 


根据 4 范 数 的 性 质 品 LA >A 时 ,有 


— exp(CA — A0 T) 


1 
| nC) ;< d -—À le; Go ll, 


Du 


1 — exp( CA AJT? 
len la < (lr, —CBL .+ I CABE | o 997—777) lec ll; — elle co a 


定义 
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L= —AT) 
p= || Ia — BL || -+ || CABL | . LARMET 


当 》 Rt LC 17 o a e A He (2) M 


(5. 43) 


pl 
M i 一 co 时 ,有 


le; 一 0，zE[0,T] 
由 po 的 定义 可 知 , 当 取 二 =(CB) ! 时 , || E, —CBL || .= 二 0,p 的 值 最 小 ,收敛 速度 最 快 。 


5.5.3 仿真 实例 
考虑 多 输入 多 输出 非 线性 系统 : 


MAL ue á NAE 1 heit 
TS, l lj iz; O lj lair) 
yi O9 u 2 Ql lax Ch) 
MME | | n 


期 望 跟踪 轨迹 为 


yida(zt) | |l. 5t 
pal hal diis 
2 g f 0.5 一 1 
hTca-[ || AO L- cea =P MELLE 
根据 式 (5. AD RISK C5. 420 ,学 习 控制 律 及 初始 状态 学 习 律 分 别 为 


Ugep Ct) iu tt) 0.4 —0.5] [er C) 
ed "T wl * P 0.5 | NP 
p | " E a y 4 0 | wes 
22633) CO) X050) 0 0.5] leny C0) 


tico C) 0 28105 C0) 2 
系统 的 初始 控制 输入 为 | | 一 H ,系统 的 初始 状态 为 | | ex l 仿真 结 


uzo (t) 


果 如 图 5-10 一 图 5-15 所 示 。 


X20 CO) 1 : 


yld,yl 


y2d,y2 


0 0.2 0.4 


6 0.8 1 


0. 
time(s) 
图 5-10 5 次 迭代 对 象 输出 的 跟踪 过 程 
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1 [^ 
[4 0 
ic 
a 一 

-2 
0 0.2 0.4 0.6 0.8 ] 
time(s) 

e 
六 
d 
A 
- 


0 0.2 0.4 0.6 0.8 1 
time(s) 
图 5-11. 5 次 迭代 后 正弦 位 置 跟 踪 


change of maximum absolute value of errorl and error2 with times i 
25 


t2 
e 


error] and error2 


0 05 1 L5 2 25 3 35 4 45 35 
time(s) 


图 5-12 5 次 迭代 过 程 中 误差 范 数 的 收敛 过 程 


time(s) 


5-13. 10 次 迭代 对 象 输出 的 跟踪 过 程 
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yld,yl 
e 


y2d.y2 
e 


0 0.2 0.4 0.6 0.8 
time(s) 


图 5-14. 10 次 和 迭代 后 正弦 位 置 跟踪 


change of maximum absolute value of errorl and error2 with times i 
25 


error] and error2 


图 5-15. 10 次 和 迭代 过 程 中 误差 范 数 的 收敛 过 程 


仿真 程序 如 下 : 
d) 主 程序 ; chap5 3. m, 


% Learning Control with an arbitrary initial state 
clear all; 


close all; 
global A B 


A-2[-23;11]; 
B-[11;01]; 
C- [20;0 1]; 
L= inv(C* B); 


ts= 0.01; 
fork=1:1:101 
ul(k) = 0;u2(k) = 0; 


end 
xk0 = [2;1]; 
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%xk0=[-2;-1]; 
LLLLILLLLILILLLILLLLILLLLEELLLEELLLRELLLELLELREELELLLLLLLELLLLLLLLIRLI 
M= 10; 
for i-0:1:M * Start Learning Control 
i x 
pause(0. 005); 
ifi-- 

xki = xk0; 
else 

yd0= 0; 

yi0= [2 * xk0(1) ; xk0(2) ]; 

e0 = yd0 - yi0; 

xki= xk0 + B*L*e0; 
end 


xk0 = xki; 用 xko 存储 上 次 运行 的 初始 状态 


DLLILILILLILLLILLLLILELLLLLLLLLLLLRLELELLLLLELLLLLLELELLLLLLLLEI 
for k-21:1:101 
time(k) = (k- 1) * ts; 


S722; 

if S==1 
yld 1(k)21.5* (k-1)*ts; 
y2d 1(k)21.5*(k-1)*ts; 
yld(k)21.5*k»*ts; 
y2d(k) »1.5*k* ts; 

elseif S == 
yld 1(k) = sin(4 * pi* (k- 1) * ts); 
y2d_1(k) = sin(4 * pi * (k- 1) * ts); 
yld(k) = sin(4 * pi * k * ts); 
y2d(k) = sin(4 * pi * k * ts); 

end 


TimeSet = [(k- 1) * ts k * ts]; 

para = [u1(X) ;u2(k)]; 

if k== % Initial state at times M 
xk = xk0; 

end 


yl l(k) 2» 2 * xk(1); 

y2_1(k) = xk(2); 

5 xk 

[tt,xx] = ode45('chap5 3plant',TimeSet, xk, [ ], para) ; 
$% xx 

xk = xx(length(xx), :); 

yl(k) 7-2 * xk(1); 

y2(k) = xk(2); 


el 1(k)= yld 1(k) - y1 1(k); 
e2_1(k) = y2d 1(k) - y2 1(k); 
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el(k) = yld(k) - y1(X); 
e2(k) = y2d(k) - y2(k); 


del(k) = (el(k)- el 1(k))/ts; 

de2(k) = (e2(k) - e2 1(k))/ts; . 

dek = [del(k);de2(k)]; 

Uk= [u1(k);u2(k)]; . 

U= Uk + L * dek; * Control law: Uk is U(i- 1), dek is near to de(i- 1) 


ul(k) = U(1); 
u2(k) = U(2); 
end % End of k 


figure(1); 

subplot(211); 

hold on; 

plot(time, yld_1, 'r', time, yl_1, 'b'); 
xlabel('time(s)');ylabel('yld, y1"); 


subplot(212); 

hold on; 

plot( time, y2d 1, 'r', time, y2_1, 'b'); 
xlabel('time(s)');ylabel('y2d, y2'); 


i=1i+1; 
times(i)-i-1; 

eli(i) = max(abs(el 1)); 
e2i(i) = max(abs(e2 1)); 


end * End of i 

T DE E T B B T E T T T T b T TT DTD TD T T T L T E T T T E b E D D T D T B B T B b T E T D T D L b K RE 
figure(2); 

subplot(211); 


plot(time, yld 1, 'r',time,yl 1, 'b'); 
xlabel('time(s)');ylabel('yld, y1'); 
subplot(212); 

plot(time, y2d 1, 'r',time,y2 1, 'b'); 
xlabel('time(s)'); ylabel('y2d, y2"); 


figure(3); 

plot(times,eli,'* — r',times,e2i, 'o—- b'); 

title('change of maximum absolute value of errorl and error2 with times i'); 
xlabel('times'); ylabel('errorl and error2'); 


(2) 子 程序 ; chap5 3plant. m, 


function dx = PlantModel(t, x, flag, para) 
global A B 
dx = zeros(2,1); 


u= para; 
dx-A*x*t*B*u; 
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实际 工程 中 ,机 械 手 常 在 有 限时 间 内 执行 重复 性 的 控制 任务 。 针 对 这 种 有 限 区 间 执 行 


重复 性 的 控制 任务 ,迭代 学 习 控制 是 一 种 有 效 的 控制 方法 。 因 此 ,机 械 辟 轨迹 跟踪 的 迭代 学 
习 控 制 得 到 广泛 研究 。 


常规 的 机 械 手 迭 代 学 习 控 制 方法 需要 假设 机 械 臂 初始 状态 与 期 望 轨 迹 初始 状态 相等 ， 


而 这 在 实际 过 程 中 常常 无 法 满足 ,因而 针对 带 有 初始 角度 偏 移 的 机 械 手 迭代 学 习 控 制 问题 
的 研究 具有 一 定 工 程 意 义 ""]。 


[14] 


[15] 
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机 械 手 反 演 控制 


CHAPTER 6 


6.1 简单 反 演 控制 器 的 设计 


反 演 (backstepping) 设 计 方 法 的 基本 思想 是 将 复杂 的 非 线 性 系统 分 解 成 不 超过 系统 阶 
数 的 子 系统 ,然后 为 每 个 子 系统 分 别 设计 李 雅 普 诺 夫 函 数 和 中 间 虚 拟 控 制 量 ,一 直 “ 后 退 ” 到 
整个 系统 ,直到 完成 整个 控制 律 的 设计 。 

反 演 设计 方法 ,又 称 反 步 法 、 回 推 法 或 后 推 法 ,通常 与 李 雅 普 诺 夫 型 自 适应 律 结合 使 用 ， 
综合 考虑 控制 律 和 自 适应 律 ,使 整个 闭环 系统 满足 期 望 的 动 .静态 性 能 指标 。 


6.1.1 基本 原理 
假设 被 控 对 象 为 


T —E,--$; 
(6.1) 
ir. f st) g(r st 
HP, gelr 0, 
控制 目标 是 使 系统 的 输出 xz, 可 以 很 好 地 跟踪 系统 的 期 望 轨迹 xz, 并 且 所 有 的 信号 
有 界 。 
定义 角度 误差 
z; =T; za 
则 
zy =t =z =t Feza 
基本 的 backstepping 控制 方法 设计 有 以 下 2 个 步骤 ， 
(1) 定义 Lyapunov 函数 为 


1 


Vi 一 本 2 6. 2) 


则 


Vi =z =z Gi 十 二 一 证 


取 t= =z mez Fr Fz E E T ES 为 虚拟 控制 量 , 即 
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zs 二 Xi1 十 Xs 十 C1z1 一 之 4 
则 
Vi=—cizi 十 zizs 
如 果 z, 二 0, 则 VV 过 0。 为 此 ,需要 进行 下 一 步 设 计 。 
(2) 定义 Lyapunov 函数 为 
V; Vi td (6.3) 
BDFoz.—cadbfGs.0-cgG Du-cz,—2z,. M 
V, =V, d zza 
=—cizi Hziz: +z:(xı +x: +f(x,t)+g(z,t)u+ciži —ža4) 
为 使 V, 志 0, 设 计 控 制 器 为 
u ==> ti =r = f(Gr,t)—cQz,-F£z4,—c0,2;— 2) (6.4) 
g Gr t) 
其 中 ,c, 为 大 于 零 的 正常 数 。 
于 是 


V,——c,z!—c,21x0 
E z t 1 t 
即 六 一 一 7:, 积 分 得 | y,dVi— -[ 7 dt , Ji 
D 2 0 


InV., 


Q7 
其 中 ,7 一 2max(Cciycs)。 
从 而 得 到 指数 收敛 的 形式 
V,G)-—V,(QODe 
由 于 V, eiti zı 和 z: 指数 收敛 , 且 当 + 一 ce 时 ,xz 一 0 和 zz 一 0s 又 由 于 


z57x4- 5-642, zaa W La Hra >z a MAM X2 AR. 


6.1.2 仿真 实例 
被 控 对 象 的 动态 方程 如 下 : 
rr um 
: | gsinz, —mix$cosr,sinz;/Cm,--m) cosz;/Gmn,d-m) 
dn 1[4/3—mcos' x, / Gn, 4-m)] I[4/3—mcos'z,/(m.3-m)]. 


Ru, Xs S.Ig—9.8m.—1.m-—0.1.1—0.5.u 为 控制 输入 。 
位 置 指令 为 zs(t)==0. 1sin(xt), 采 用 控制 律 式 (6.4), 取 ci 二 35,cs 二 35, 系 统 初始 状态 
为 Lx/60,0]。 仿 真 结果 如 图 6-1 和 图 6-2 所 示 。 
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o 
Uo 


ideal position 
position tracking 


o 
N 


e 


position tracking 


> 


position tracking error 
b 5 5 e 
o 9 2 
一 N N 


S 


0 1 2 3 4 5 6 T 8 9 10 
time(s) 
6-1 位 置 跟踪 及 跟踪 误差 


10 


control input 


1 2 3 4 5 6 7 8 9 10 
time(s) 
图 6-2 控制 输入 
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仿真 程序 如 下 : 
(1) Simulink 主 程序 : chap6. 1sim. mdl。 


To Workspace10 


chap6_1plant 


S-Function1 


Sine Wave 


To Workspace7 


Clock To Workspace 


(2) 控制 器 子 程序 chap6_1lctrl. m, 


function [sys,x0, str,ts] = spacemodel(t, x, u, flag) 
switch flag, 
case 0, 
[sys, x0, str, ts] = ndlInitializeSizes; 
case 3, 
sys = ndlOutputs(t, x,u); 
case (2,4,9) 
sys- []; 
otherwise 
error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0, str, ts] = mdlInitializeSizes 


sizes = simsizes; 


sizes.NumContStates 


sizes.NumDiscStates 


sizes.NumOutputs 


sizes.NumInputs 


Li 
oO |^ Urn ccióo 


sizes.DirFeedthrough 


sizes.NumSampleTimes 
sys = simsizes(sizes); 

x0 = []; 

SEs = []; 

ts = [1 

function sys = mdlOutputs(t, x, u) 
zd- u(1); 

dzd= 0.1 * pi * cos(pi * t); 

ddzd= - 0.1 * pi* pi% sin(pi*t); 


xl =u(2); 
x2 = u(3); 
fx-u(4); 
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gx= u(5); 
c1 = 35;c2 = 35; 


zl-7xl- zd; 

z2 = x1 + x2 + c1 * z1 - dzd; 

dzl = x1 + x2 - dzd; 

ut = ( — x1 — x2 — fx- c1 * dzl + ddzd- c2 * z2 - z1)/(gx + 0.001); 
sys(1) = ut; 


(3) 被 控 对 象 子 程序 chap6, 1plant. m, 


function [sys, x0, str,ts] =s_function(t, x, u, f lag) 
switch flag, 
case 0, 
[sys, x0, str, ts] = ndlInitializeSizes; 
case 1, 
sys = ndlDerivatives(t, x,u); 
case 3, 
sys = ndlOutputs(t,x,u); 
case (2, 4, 9 } 


sys = []; 
otherwise 

error(['Unhandled flag = ',num2str(flag)]); 
end 


function [ sys, x0, str, ts] = mdlInitializeSizes 


sizes = simsizes; 


sizes. NumContStates 


sizes. NumDiscStates 
sizes.NumOutputs 


sizes.NumInputs 
sizes.DirFeedthrough 
sizes. NumSampleTimes 


LU 
oOo0on- Boe 
T 


Sys = simsizes(sizes); 

x0 = [pi/60 0]; 

str-[]; 

ts- []; 

function sys = mdlDerivatives(t,x,u) 

g79.8;nc-1.0;m-20.1;1-72 0.5; 

S=1x (4/3-m* (cos(x(1)))^2/(mc* m)); 
fx-g*sin(x(1))-m*1*x(2)^2* cos(x(1)) * sin(x(1))/(mc + n); 


fx- fx/S; 
gx = cos(x(1))/(mc +m); 
gx - gx/S; 
ut - u(1); 


sys(1) 7» x(1) + x(2); 

sys(2) = fx + gx * ut; 

function sys = mdlOutputs(t,x,u) 

g79.8;mc-1.0;m-20.1;17 0.5; 

S-1*(4/3-nm* (cos(x(1)))^2/(mc + m)); 
fx-g*sin(x(1))-m*1»*x(2)^2*cos(x(1)) * sin(x(1))/(mc + n); 
fx- fx/S; 
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gx = cos(x(1))/(mc+m); 
gx = gx/S; 
sys(1) » x(1); 

sys(2) = x(2); 
sys(3) = fx; 

sys(4) = ax; 


(4) 作 图 程序 : chap6. 1plot. m, 


close all; 


figure(1); 

subplot(211); 

plot(t, yd( :,1), 'r',t, y( :, 1), 'k:', 'linewidth',2); 
xlabel('time(s)');ylabel('position tracking'); 
legend('ideal position', 'position tracking'); 
subplot(212); 

plot(t, yd(:,1) = y( :, 1), 'k', 'linewidth',2); 
xlabel('time(s)');ylabel('position tracking error'); 


figure(2); 
plot(t,ut(:,1), 'r', 'linewidth',2); 
xlabel('time(s)');ylabel('control input'); 


6.2. 单 关 节 机 械 手 的 反 演 控制 


反 演 控制 的 设计 方法 实际 上 是 一 种 逐步 递 推 的 设计 方法 , 反 演 控制 设计 方法 中 引进 的 
虚拟 控制 本 质 上 是 一 种 静态 补偿 思想 ,前 面 的 子 系统 必须 通过 后 边 子 系统 的 虚拟 控制 才能 
达到 目的 ,因此 该 方法 要 求 系统 结构 必须 是 严格 参数 反馈 系统 或 可 经 过 变换 ,转化 为 该 种 类 
型 的 非 线 性 系统 。 反 演 控制 设计 方法 在 设计 不 确定 性 系统 (特别 是 当 干 扰 或 不 确定 性 不 满 
足 匹 配 条 件 时 ) 的 和 鲁 棒 控 制 器 或 自 适应 控制 器 方面 已 经 显示 出 它 的 优越 性 。 


6.2.1 系统 描述 
采用 电机 驱动 的 单机 械 臂 进行 仿真 ,系统 的 动态 方程 如 下 : 
qu ume 
z; = ir tuf a tap 
4 ' » j i (6.5) 
Es =—Tz; PAZ ti 
y =L] 


其 中 ,x —0,2,; —0 x, —1,M,—J pm +M? DN — mgl -Mgl s 为 重力 加 速度 常 


ht. f 为 已 知 非 线 性 函数 ,0 为 连 杆 角度 ,I 为 电流 ,K, 是 扭矩 常量 ,KK 是 反 电 动 势 系数 ,B 
是 轴承 黏 滞 摩擦 系数 ,D 是 负载 直径 ,! 是 连 杆 长 度 ,M 是 负载 质量 ,m 是 连 杆 重量 ,L 是 电 


第 6 章 ”机械 手 反 演 控制 ue 157 


抗 ,R 为 电阻 ,wu 为 电机 控制 电压 ,J 为 执行 器 转 矩 。 
控制 目标 是 使 系统 的 角度 输出 x, 跟踪 的 期 望 轨迹 zu. fA EHE c. 跟踪 期 望 速度 轨迹 
zu， 并 且 所 有 的 信号 有 界 。 


6.2.2 反 演 控制 器 的 设计 
被 控 对 象 可 写 为 


EA — E» 
Z =at talr) Farta 

(6.6) 
Za 一 OZs 十 bzs bau 


Y =R] 


Ahasi eS Gn sro ambi pm — sr. 
上 述 模型 属于 非 匹 配 系统 ,很 难 采 用 传统 的 控制 方法 (如 滑 模 控制 方法 ) 设 计 控 制 律 , 适 
合 采 用 反 演 控制 方法 进行 设计 。 定 义 角 度 误 关 z Sr zu UU 
Zi =t; Z4 =T: žų 
反 演 控制 的 设计 过 程 是 通过 逐步 构造 中 间 量 完成 的 ,最 后 的 虚拟 控制 量 是 施加 于 系统 
实际 控制 量 的 一 部 分 。 针 对 模型 式 (6. 6) 的 反 演 控制 方法 设计 步骤 如 下 。 
d) 定义 Lyapunov FR 


l 2 
Vi oe 


则 
V, -euz;exeQ(r.—234) 
取 Z 三 一 cizi 十 zy 十 zs, 其 中 ,ci 二 0,z， 为 虚拟 控制 量 , 即 


Z =Z Fizi 一 之 4 (6. 7) 
Du 


V,—-—ec,zi-czz, 
如 果 2, —0, 9M 人 和 0。 为 此 ,需要 进行 下 一 步 设计 。 
(2) 定义 Lyapunov PA% 
1 
V,=Vi 十 二 xz? 

2 

HF z;—a,rQ;dTas;Go T a4a44- cz ,— z, M 
V,—V,-Fz,£,— —ce zio ziz: +z: laz: +a: (r)--a,2, +c; — ža) 

Wax; =—aixzı — alx) cizi H2 caz: zi Hz APs >0,z; 为 虚拟 控制 量 , 即 


Z; =at; Faito taz (x) + cz 一 2 十 czza 十 2 (6.8) 
则 


Vi=Vi 二 zzs==cz1!— Ce? -£,24 
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如 果 z,—0. 0 V; 委 0。 为 此 ,需要 进行 下 一 步 设计 。 
第 3 步 : 5E X. Lyapunov 函数 


HT zy =aäszy Faixa Faal) teri zatez zs 
V, —V,--z,2, =— ciz? — cri b zz. + 
zs 区 (bin Hbr: + pues d, Gr) - c£, —E, + cid «a 
4^ T-—a, (biz bans tuu) , 则 


V,— —c,z!—c,zi-c zz; Fe (T Hata tåalt) teti yF +) 


为 使 V, 志 0, 设 计 控制 器 为 
T =at d(C = 4 —6$424 (6. 9) 
其 中 ,cs 为 大 于 零 的 正常 数 。 
实际 的 控制 律 为 

u =L (FT -b,x — b,x) (6. 10) 
则 

Vs=—cz!—cszi—csz! [L0 
即 

V,« 39V, 


其 中 ,7 一 2min(cl 0; c). 

引 理 6.1 针对 V; [0,99 € R RERS BE VC aV - f N (24270 的 解 为 

VE EE "Vn? +| e "7? f(r)dr 

其 中 ,a 为 任意 常数 。 l 

采用 引 理 6. 1, RERNE V.S gV ,有 a. f 二 0, 解 为 

V.G)Ee VVG) 

可 见 ,Vs 指数 收敛 至 零 ,收敛 速度 取决 于 7。 

dT V.—litelipRlen Wess, 和 es 指数 收敛 , 且 当 tco 时 ,<i-0vzs-0， 
z; >0, MM Ti Zast: Zao 

又 由 于 z, =z: + cizi — Zarz, =a;x; Hax, talz) b eZ, — ža Ferz: tzit = 
taza M x, "HA. 


6.2.3 仿真 实例 
针对 被 控 对 象 式 (6.5) , 取 期 望 轨 迹 za 二 sint, 非 线性 函数 为 f(x) 二 x? 十 x:。 单 机 械 
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BSX B —0.015.L —0.0008, D —0. 05. R —0.075.m —0.01,J —0.05.7—0.6, K,— 
0.085,M —0.05, K; =la g —9.8. 

系统 的 初始 状态 为 x(0) 二 [0.5,0,0] ,控制 器 参数 取 0, — 500.0, =c; — 10. f AER 
用 虚拟 控制 式 (6.7) .虚拟 控制 式 (6. 8) 及 实际 控制 律 式 (6. 10) ,仿真 结果 如 图 6-3 和 
图 6-4 所 示 。 


angle tracking 


0 5 10 15 20 25 30 
time(s) 


2 = 


angle speed tracking 
o 


0 5 10 15 20 25 30 
time(s) 
6-3 角度 和 角速度 跟踪 


0.1 


control input 
w 


0 5 10 15 20 25 30 
time(s) 


图 6-4 控制 输入 信号 
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仿真 程序 如 下 : 
(1) Simulink 主 程序 : chap6_2sim. mdl, 


Sne Wave 
SFuncbon 


(2) 控制 器 S PR: chap6_2ctrl. m, 


function [sys,x0,str,ts] = spacemodel(t, x,u, flag) 
switch flag, 
case 0, 
[sys, x0, str, ts] = ndlInitializeSizes; 
case 3, 
sys = mdlOutputs(t, x, u); 
case (1,2,4,9) 
sys- []; 
otherwise 
error(['Unhandled flag = ',num2str(flag)]); 
end 


function [sys, x0, str, ts] = mdlInitializeSizes 


Sizes - simsizes; 


sizes.NumContStates 


sizes.NumDiscStates 
sizes.NumOutputs 


sizes.NumInputs 


sizes.DirFeedthrough 


I 
O e Aà HOO 
~ 


sizes. NumSampleTimes 
Sys = simsizes(sizes); 

xo = [] 

str = []; 

ts = [] 

function sys = mdlOutputs(t, x, u) 

B=0.015;L= 0.0008;D= 0.05;R= 0.075;m= 0.01;J= 0.05; 
1=0.6;Kb= 0.085;M= 0.05;Kt=1;g= 9.8; 
Mt-J4*1/3*m*1^24*1/10* M* 102* D; 
N2m*g*1*M*g*]1]; 


zd- u(1); 
dzd = cos(t); 
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ddzd= - sin(t); 


xl =u(2); 
x2 = u(3); 
x3 = u(4); 


Ix-zxl^2t4 x2^2; 


zl-xl- zd; 
c1 = 500;c2 = 10;c3 = 10; 


al = - B/Mt;a2 = N/Mt * fx;a3 = Kt/Mt; 
bl = - R/L;b2 = - Kb/L;b3 = 1/L; 


dx2 = al * x2 + a2 + a3 * x3; 
dfx = 2 * x1 * x2 + 2 * x2 x dx2; 


da2 = N/Mt * dfx; 

dzl = x2 - dzd; 

ddz1 = dx2 - ddzd; 

Z2 = x2 + c1 * z1 - dzd; 

dz2 = dx2 + c1 * dzl — ddzd; 

z3 = a3 * x3 + a1 * x2 + a2 + c1 * dz1 - dddzd + c2 * z2 + z1; 

T= — al * dx2 - da2 - c1 * ddz1 + ddzd - c2 * dz2- dz1- z2- c3 * z3; 
ut-L*(1/a3* T- b1 * x3- b2 * x2); 


sys(1) = ut; 
(3) 被 控 对 象 S 函数 : chap6_2plant. m. 


function [sys,x0,str,ts] = s function(t, x,u, flag) 
switch flag, 
case 0, 
[sys, x0, str, ts] = ndlInitializeSizes; 
case 1, 
sys = ndlDerivatives(t,x,u); 
case 3, 
sys = mdlOutputs(t,x,u); 
case (2, 4, 9 } 


sys = []; 
otherwise 

error(['Unhandled flag = ',nunm2str(flag)]); 
end 


function [sys, x0, str, ts] = ndlInitializeSizes 


sizes - simsizes; 


sizes.NumContStates 


IM 


sizes.NumDiscStates 


S. 


sizes.NumOutputs 


sizes.NumInputs 
sizes.DirFeedthrough 
sizes. NumSampleTimes 


LU LU u 
O © t 0 © U 
T We 9e 


E 


Sys = sinsizes(sizes); 
x0- [0.50 0]; 
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str-[]; 

te= []; 

function sys = mdlDerivatives(t, x,u) 

B=0.015;L= 0.0008;D= 0.05;R= 0.075;m= 0.01;J= 0.05; 
1=0.6;Kb= 0.085;M= 0.05;Kt=1;g= 9.8; 

Mt =J+1/3*m* 1^2 +1/10* M=» 1^2 * D; 
N=m*g*x*l+M*gx l; 


Ix x1^2- x2^2; 

sys(1) = x(2); 

sys(2) = - B/Mt * x(2) + N/Mt * fx + Kt/Mt * x(3); 
sys(3) = - R/L * x(3) - Kb/L* x(2) + 1/L*u; 
function sys = mdlOutputs(t, x, u) 

sys(1) = x(1); 

sys(2) = x(2); 

sys(3) = x(3); 


(4) 作 图 程序 : chap6_2plot. m。 


close all; 


figure(1); 

subplot(211); 

plot(t, y(:,1), 'r',t, y( :, 2) , 'b', 'linewidth',2); 
xlabel('time(s)');ylabel('angle tracking'); 
subplot(212); 

plot(t,cos(t), 'r',t,y(:,3), 'b', 'linewidth',2); 
xlabel('time(s)');ylabel('angle speed tracking'); 


figure(2); 
plot(t,ut, 'r', 'linewidth',2); 
xlabel('time(s)');ylabel('control input'); 


6.3 WMA BH B RR RR 


双 耦 合 电机 控制 系统 是 一 种 特殊 机 械 手 的 结构 形式 ,本 节 介绍 一 类 双 耦 合 电机 控制 


系统 反 演 控制 的 设计 方法 。 
6.3.1 系统 描述 
两 个 耦合 电机 的 模型 为 
Jabi 十 ci6 Hkg; (gr 0, —0,) =u (6.11) 
Jð, 4- c,0, +k (0, — g;0,) —0 (6. 12) 
其 中 ,0, 为 驱动 器 转动 角度 ,0。 为 负载 转动 角度 ,省 为 负载 转动 惯量 ,Js 为 驱动 器 转动 惯 
lig = ORAE EE su 为 控制 输入 ,c; 为 驱动 器 阻尼 ,c, 为 负载 阻尼 ,二 2k1r? HH 


转 弹性 常数 。 
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双 电 机 耦合 运动 系统 示意 图 如 图 6-5 所 示 。 


旋转 阻尼 器 。 驱动 器 


图 6-5 双 电 机 耦合 运动 系统 示意 图 


控制 目标 是 负载 转动 角度 0, 跟踪 期 望 负载 角度 9,,, 角 速度 0， 跟踪 期 望 负载 角速度 
6, ,并 且 所 有 的 信号 有 界 。 
上 述 模型 属于 非 匹 配 系 统 , 很 难 采 用 传统 的 控制 方法 (如 滑 模 控 制 方法 ) 设 计 控 制 律 , 适 
合 采用 反 演 控制 方法 进行 设计 。 
6.3.2 反 演 控制 器 的 设计 
4E XIX 22 J e — 0,4 — 0, , 滑 模 函数 为 
六 一 CE 十 Me， 太 一 e 十 Me (6.13) 
其 中 ,4 二 0。 
则 
Jit =J (E+) =J a — Jib + J ne 
= Jiba = Fi 十 Je 
K'h.F, —],0,— —c,0,—k(,—g. 0. 
(OD 考虑 滑 模 图 数 ~, 定义 Lyapunov 函数 
1 " 
Wi 
则 
V, Jur —r(Jib4—F, FJ de ) 
取 J10w4 一 Fi 十 J Aé = 一 kir 十 z, ,zs 为 虚拟 控制 量 , 则 


V——kh,.r tzr 
其 中 ,二 0, 且 


z: =J] ð — F,-- JiAé - har (6. 14) 


如 果 x 二 0, 则 V, 壹 0。 为 此 ,需要 进行 下 一 步 设计 。 
(2) 考虑 zx, ,定义 Lyapunov 函数 
ls 


V. =V, tyn 
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则 =J Öna Fi +J Atk E 
V,=V, Tz, — —h,r! zr z: (J Öna — F: JrAet+kr ) 
取 J E HJA tkir ks = ss ,Zz3' 为 虚拟 控制 量 , 则 


V,——h,r'—hk,zidzz; 
RB.R,0.H 
z; =J —F, 4- Ji tki tkz: 十 了 
2 


F,—-—c,0, — k (Å, 一 可 一 — b (0, — g;'0,) 


1 


如 果 z; = 二 0, 则 V, 志 0。 为 此 ,需要 进行 下 一 步 设 计 。 
第 三 步 : 考虑 zx, ,定义 Lyapunov 函数 


lg 
V; =V, Tn 
则 


V,—V;--z,£,— —hk,r'—h,zi--zz,- mi. 
zs 为 虚拟 控制 量 , 且 


zs —J 0, —F, + JAë Hki +k, +r 


Efa 
3s "MEC B RENT. T E E3 ENT. 
F= TÉÍ^ k(0,—g, 0,)— Ti dux g? d.) 
取 F,—J,0,F.—c0i-- kg, (i. 0, 0: JU 
F,—J,0, =u — F, 
NN S ad d 
im na "rr. EJ." FA) 


由 式 (6.16) 可 知 , 如 果 取 
F,—J10 a - JAE AF SFR REST x az, 

其 中 ,3: 之 0。 

则 Z4, —z,—h3z, ,从 而 

V, =— kir’ — kzi — kz; 
Bp 
V. — 7V; 

其 中 :3—2max(c, 65.03). 

dst Co. 190 iT F, , 则 由 式 (6. 18) 可 得 控制 律 为 


SEE 


k Co 。 T 
DEO FF, )2- F. 


采用 6.2 节 的 引 理 6. 1 ,针对 不 等 式 方程 V, 过 一 7V: fi a. f. 二 0, 解 为 


(6.15) 


(6. 16) 


66, 17) 


(6. 18) 


(6. 19) 


(6. 20) 


(6. 21) 


可 见 ,V,(z) 指 数 收敛 至 零 ,收敛 速度 取决 于 7。 由 于 了 ,一 二 


之 2 和 之 3 指数 收敛 
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- 


VG )y&e "^ "V. Cto) 


E i 


: FT —z?, W B 
Jar T4305 SEE 


2 


, 且 当 上 一 co 时 ,e >(0 ,CE ”>0 ,之 2 >~0，za ~0。 


6.3.3 仿真 实例 


被 控 对 象 为 式 (6. 11) 和 式 (6. 12) , 取 Ji =0. 3575,74 — 0. 000425, g, — 4, c, —0. 004, 
c, —0. 05.5 —8. 45, 采 用 控制 律 式 (6. 140 . 式 (6. 15) 和 式 (6. 210, C k, — 10,5, —10, 7, —10,. [ij 
真 结果 如 图 6-6 和 图 6-7 所 示 。 


angle tracking 


angle speed tracking 


control input 


2 

1 

0 
-1 

5 10 15 20 25 30 
time(s) 
i 
-l 
-2 
0 5 10 15 20 25 30 
time(s) 
图 6-6 角度 和 角速度 跟踪 
0.4 
0.3 
0.2 
0.1 
0 
-0.1 
-0.2 
-0.3 
-0.4 
-0.5 
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time(s) 
6-7 控制 输入 
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仿真 程序 如 下 : 
(1) Simulink 主 程序 : chap6_3sim. mdl。 


To Workspace2 


chap6 3plant 


S-Function1 


3 1 chap6 3ctr 
Sine Wave 


LEE To Workspace 


Clock To Workspace 


(2) 控制 器 S 函数 : chap6 3ctrl. m, 


function [sys,x0,str,ts] = spacemodel(t, x,u, flag) 
switch flag, 
case 0, 
[sys, x0, str, ts] = ndlInitializeSizes; 
case 3, 
sys = ndlOutputs(t,x,u); 
case {2,4,9} 
sys - []; 
otherwise 
error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0,str,ts] = mdlInitializeSizes 


Sizes - simsizes; 


sizes. NumContStates 
sizes.NumDiscStates  - 
sizes. NumOutputs 三 
sizes.NumInputs = 
sizes.DirFeedthrough = 


[22 5 NN ODO 


sizes.NumSampleTimes - 
Sys 7 simsizes(sizes); 
x0 = []; 

str = []; 

ts = []; 

function sys = mdlOutputs(t, x, u) 
Jl = 0.3575;Jd = 0.000425; 

gr = 4;c1 = 0. 004;c2 = 0. 05;k = 8. 45; 


k1 = 10;k2 = 10;k3 = 10; 


th2d- u(1); 
dth2d = cos(t); 
ddth2d= - sin(t); 
dddth2d= - cos(t); 
ddddth2d = sin(t); 
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thl = u(2);dth1 = u(3); 

th2 = u(4);dth2 = u(5); 

ddth2 = — 1/J1 * (c2 * dth2 + k * (th2 — 1/gr * th1)); 
dddth2 = — 1/J1 * (c2 * ddth2 + k * (dth2 - 1/gr * dth1)); 


e- th2d - th2; 

de = dth2d - dth2; 

dde = ddth2d - ddth2; 
ddde = dddth2d — dddth2; 


nmn = 3; 
r-de*nmn*e; 

dr - dde * nmn * de; 
ddr = ddde + nmn * dde; 


F1 = —-c2*dth2-k* (th2- 1/gr * th1); 
dFl- —c2/Jl* F1 - k * (dth2 - 1/gr * dth1); 


z2 = Jl * ddth2d- F1 + Jl* nmn * de + k1* r; 
dz2 = Jl * dddth2d- dF1 + Jl * nmn * dde + k1 * dr; 


z3 = Jl * dddth2d - dF1 + Jl * nmn * dde + k1 * dr + k2 * z2 + r; 


ddF1 = Jl x ddddth2d + J1 * nmn * ddde + k1 * ddr + k2 * dz2 + dr + z2 + k3 * z3; 
F3 = c1 * dth1 + k * 1/gr * (1/gr * thl — th2); 


ut = gr * Jd/k * (k/J1 x F1 + c2/J1 x dF1 + ddF1) + F3; 


sys(1) = ut; 
(3) 被 控 对 象 S 函数 : chap6 3plant. m, 


function [sys, x0, str, ts] = s function(t, x, u, f lag) 
switch flag, 
case 0, 

[sys, x0, str, ts] = ndlInitializeSizes; 
case 1, 

sys = ndlDerivatives(t,x,u); 
case 3, 

sys = mndlOutputs(t,x,u); 
case (2, 4, 9] 

sys = []; 
otherwise 

error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0, str, ts] = ndlInitializeSizes 
sizes = simsizes; 
sizes.NumContStates  - 


4 
sizes.NumDiscStates = 0; 
sizes. NumOutputs =4 
1 


sizes.NumInputs = 
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sizes.DirFeedthrough 
sizes.NumSampleTimes 


"Wow 
Oo "m 
e "e 


Sys = simsizes( sizes); 
x0-[0.500.50]; 

str-[]; 

ts-[]; 

function sys = mdlDerivatives(t,x,u) 
Jl = 0.3575;Jd = 0.000425; 

gr = 4;c1 = 0. 004;c2 = 0. 05;k = 8. 45; 


thl = x(1);dthl = x(2); 
th2 = x(3) ;dth2 = x(4); 


ut-u(1); 


S1 = 1/Jd * (ut- c1 * dthl — k * 1/gr * (1/gr * thl - th2)); 
S2- —1/J1* (c2 * dth2 +k * (th2 - 1/gr * th1)); 


sys(1) = x(2); 


sys(2) = S1; 
sys(3) = x(4); 
sys(4) = S2; 


function sys = ndlOutputs(t, x, u) 
sys(1) - x(1); 
sys(2) = x(2); 
sys(3) = x(3); 
sys(4) = x(4); 


(4) 作 图 程序 : chap6. 3plot. m, 


close all; 


figure(1); 

subplot(211); 

plot(t,sin(t), 'r',t,y(:,2), 'b', 'linewidth',2); 
xlabel('time(s)');ylabel('angle tracking'); 
subplot(212); 

plot(t,cos(t), 'r',t, y(:,3), 'b', 'linewidth',2); 
xlabel('time(s)');ylabel('angle speed tracking'); 


figure(2); 
plot(t,ut, 'r', 'linewidth',2); 
xlabel('time(s)');ylabel('control input'); 
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机 械 手 滑 模 控 制 


CHAPTER 7 


7.1 机 械 手 动力 学 模型 及 特性 


一 个 典型 的 多 关节 机 械 手 如 图 7-1 所 示 。 
考虑 一 个 n 关节 机 械 手 ,其 动态 性 能 可 由 二 阶 非 线 性 微分 方程 描述 : 
M (q)0d 4- C(q.q)qd +G) HEG) +r —c (.1) 
式 中 ,gq ER” 为 关节 和 角 位 移 量 ,M(Cqg)ER"… 为 机 械 手 的 惯性 矩阵 ,CCq,qg)ER" 表示 离心 
力 和 哥 氏 力 ,G(qg)ER" 为 重力 项 ,F(q ) ER 表示 摩擦 力矩 ,r € R" 为 控制 力矩 ,ruER" 为 
外 加 扰动 。 
机 械 手动 力学 特性 如 下 : 
A) M(q) 一 2C(q ,9 ) 是 一 个 斜 对 称 和 矩阵。 
(2) 惯性 矩阵 M(qg) 是 对 称 正 定 矩 阵 ,存在 正 数 m, 、m, ,满足 如 下 不 等 式 : 
mı || x ||? < x'M(qpxsm;lxl* (7.2) 
(3) 存在 一 个 依赖 于 机 械 手 参数 的 参数 向 量 ,使 得 M (D C GL 4D GU) .F CO i JE X 
M (q09 -- C(q.dDp +G) -- F(q) =P (q.q.p.0)P (7.3) 
HB. (q.q ,p,9)ER” ”为 已 知 关 节 变 量 函 数 的 回归 和 矩阵 , 它 是 机 械 手 广义 坐标 及 其 各 
阶 导数 的 已 知 函 数 和 矩阵 .PER" 是 描述 机 械 手 质量 特性 的 未 知 定常 参数 向 量 。 
一 个 典型 的 双关 节 刚 性 机 械 手 示意 图 如 图 7-2 所 示 , 本 书 中 的 大 多 数 仿真 实例 都 采用 
该 机 械 手 进行 验证 。 


图 7-1 一 个 8 关节 机 械 手 图 7-2 双关 节 刚 性 机 械 手 示意 图 
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7.2 基于 计算 力矩 法 的 渭 模 控制 

计算 力矩 法 是 机 械 手 控制 中 较 常 用 的 方法 品 , 该 方法 基于 机 械 手 模型 中 各 项 的 估计 值 
进行 控制 律 的 设计 。 

7.2.1 系统 描述 


机 械 手 的 模型 为 
M(q)0j - C(q.d)4d -G(q) —c (7.4) 
其 中 ,M(g) 为 正定 质量 惯性 矩阵,C(q ,9 ) 为 哥 氏 力 离心力,G(d ) 为 重力 。 


7.2.2 控制 律 的 设计 
当 不 知道 机 械 手 模型 的 惯性 参数 时 ,根据 计算 力矩 法 , 取 控制 律 为 


r—M(QDv-4- C(GQ.4)4-- GG) (7.5) 
其 中 ,M(q).C(q,4) 和 G(g) 为 利用 惯性 参数 估计 值 5 计算 出 的 M.C 和 G 估计 值 。 
闭环 系统 方程 为 
Mä -CQ.d)d - GIG) = MD v - aG) (7. 6) 


则 
MGpDj —M(qv-4- qii +a) —CQG.4)4 — GG) 
—M(q)v— C(q.q)d — G(q) 
将 上 式 两 边 分 别 减 去 Mq ,得 
Mj — Mv — [MY + € (404 + GGO] 


—M(pDv —Y(q.d.d)p 0, T) 


KB. M—M—-M.C-c-6.6-G-6.p—p— p. 
若 惯性 参数 的 估计 值 广 使 得 态 (g STRE DULL REESE CT. 7) 可 写 为 
d—v—[MQD0]"YQ.d.d)p —v—9 (d.d. pp ERO 
定义 
9 (q.q.d.p)p —d 
其 中 38 二 [a d.d, T d —[d, rda AA. i a 


取 滑 模 函 数 为 
s—é-F-Ae 
其 中 ye =q gq ,e 一 9 一 9 3 一 [si 5 HIE XY fA E E o 
则 


$—é--Aé—(d,—D -Aé—d,— v-d -Aé 


v=ğa tA +y (7.9) 
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式 中 ,d 为 待 设计 的 向 量 。 


则 
s=d—y, (7. 10) 
选取 
yu 一 (d 十 1)sgn(s)， ldl «d (7.11) 
其 中 ,y >00. 
定义 Lyapunov KX: 
V 一 了 ss 


则 

V —$s =(d—d)s=ds —dsgn(s)s —93sgn(s)s &—9|s | «0 
根据 LaSalle 不 变性 原理 , 当 V 夺 0 Bf .s—0, i34 t—o58f ,s—0, JA ifl e 一 0,6 一 0。 
由 式 (7.5) 和 式 (7. 9) ,得 滑 模 控制 律 为 


tr=M(g)v+ qii + GG) (7.12) 


HP, v= HA è - y, y, (d t 3) sgn(G), 
由 控制 律 式 (7. 12) 中 的 ys 表达 式 可 知 , 若 参数 估计 值 p 越 准 确 , 则 上 ‖p | 越 小 ,d 越 


小 , 滑 模 控制 产生 的 拌 振 越 小 。 


7.2.3 仿真 实例 


选 二 关节 机 械 手 力 臂 系统 ,其 动力 学 模型 为 
M(q)9 十 C(qg,q)g 十 G(q) 十 下 (dg) —- vc, —c 
其 中 wg 二 [gi ga] se = 二 [ti nl. 


取 
-F2ecos(q;) --2g9sin(q;) f--ecos(q;) + FM 


a 
M(g)= 
Becos(qs) nsin(g;) B 


(—2esin(gs) 十 2ycos(qs))q，。 (一 esin(qy) 十 TUM 


(esin(q;) — ycos(q;))q, 0 


GU) = gem 十 qz) 十 yezsin(qi 十 qz) 十 (a 一 B 十 e1)escos(qi | 
ee; cos(q, tqz) + gessin(q, 十 qz) 
HH, a= I om HI tml mul, B — I + mule = mulscos (0.5, p 三 
ml sin(0,) , 
机 械 臂 的 实际 物理 参数 值 见 表 7-1. 
表 7-1 机 械 臂 的 实际 物理 参数 值 
I./kg 9. e, e; 


m,/kg l,/m Li I,/kg m,/kg l,,/m 
2/5 0 —17/12 9.81 


1 1 1/2 1/12 3 1 
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模型 初始 值 为 [0 o 0 0], 采 用 滑 模 控制 律 式 (7. 12), 取 角度 指令 分 别 为 qa = 
. , : 25 ü 
cos(n£) sqa —sin(xt) ,M —0. 6M ,C —0. 6C,G —0.6G.d —30,5—0.10,A = | M | 2 


5 
真 结果 如 图 7-3 和 图 7-4 所 示 。 


一 2 
所 
‘6 
S 
o 
AAA 
4 
E 
L^] 
E 
& -2 
0 2 4 6 8 10 
time(s) 


angle tracking of joint 2 
N 


I 
VAN / N 
一 ] 
0 2 8 10 
time(s) 


(a) ILH S ff o FERRE 


angle speed tracking of joint 2 angle speed tracking of joint 1 
人 tb © t2 A Ja b c N 上 


time(s) 


(b) 双 机 械 臂 角速度 跟踪 
图 7-3 双 机 械 禹 角度 和 角速度 跟踪 
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1000 
[ 
E 
E 
time(s) 
500 
N 
5 
全 
i 0 
g 
—500 人 
2 4 6 8 10 
time(s) 
E 7-4 双 机 械 臂 控制 输入 
仿真 程序 如 下 : 


(1) Simulink 主 程序 : chap7_1sim. mdl。 


To Workspace1 
Sine Wave 
tina -— J i 


To Workspace3 


Sine Wave1 


(2) 控制 律 子 程序 : chap7_1lctrl. m, 


function [sys,x0, str,ts] = spacemodel(t,x, u, flag) 
switch flag, 
case 0, 

[sys, x0, str, ts] = mndlInitializeSizes; 
case 3, 

sys = ndlOutputs(t,x,u); 
case {2,4,9} 

sys- []; 
otherwise 

error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0, str, ts] = mdlInitializeSizes 
global nmn 
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nmn = 25 x eye(2); 

Sizes = simsizes; 
sizes.NumContStates = 0; 
sizes.NumDiscStates - 0; 
sizes.NumOutputs = 2; 
sizes.NumInputs = 6; 


sizes.DirFeedthrough = 1; 
sizes.NumSampleTimes = 1; 

sys 7 simsizes(sizes); 

x0 = []; 

str = [T 

ts = [00]; 

function sys = mdlOutputs(t, x, u) 
global nmn 

qdl = u(1); 


dqdl = - pi * sin(pi*t); 
ddqdl = - pi^2 * cos(pi* t); 
qd2 = u(2); 

dqd2 = pi * cos(pi * t); 
ddqd2 = - pi^2 * sin(pi* t); 
ddqd = [ ddqd1 ; ddqd2] ; 


dad = [dqd1 ; dad2] ; 
ddqd = [ddqdl ;ddqd2 ] ; 


ql = u(3);dq1 = u(4); 
q2 = u(5);dq2- u(6); 
dq = [dql;dq2]; 


el = qdi- ql; 

e2 - qd2- q2; 
e= [el;e2]; 
del = dqd1 - dq1; 
de2 = dqd2 - dq2; 
de = [de1;de2]; 


alfa-6.7;beta- 3.4; 
epc = 3. 0;eta = 0; 


m1-71;11721; 

lcl- 1/2;11- 1/12; 

g79.8; 
el-ml1*1l1*1lcl- Il1-ml* 11^2; 
e2 = g/11; 


M= [alfa + 2 * epc * cos(q2) * 2 * eta * sin(q2), beta + epc * cos(q2) + eta * sin(q2); 
beta * epc * cos(q2) * eta * sin(q2), beta]; 

C-[(-2* epc * sin(q2) * 2 * eta * cos(q2)) * dq2, ( - epc * sin(q2) + eta * cos(q2)) * dq2; 
(epc * sin(q2) - eta * cos(q2)) * dq1, 0]; 

G= [epc * e2 * cos(q1 * q2) + eta * e2 * sin(ql * q2) + (alfa- beta + el) * e2 * cos(q1); 
epc * e2 * cos(q1 + q2) + eta * e2 * sin(ql + q2)]; 

M0-0.6*M; 

C0-720.6*C; 

G0-0.6*G; 


s= de + nmn * e; 

d up = 30; 

xite - 0.10; 

xite d= (d up + xite) * sign(s); 
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v = ddqd + nmn * de + xite d; 
tol- MO * v + CO * dq + G0; 


sys(1) = tol(1); 
sys(2) = to1(2); 


(3) 被 控 对 象 子 程序 : chap? 1plant. m, 


function [sys, x0, str, ts] = s function(t, x, u, flag) 
switch flag, 


case 0, 

[sys, x0, str, ts] = ndlInitializeSizes; 
case 1, 

Sys = ndlDerivatives(t, x,u); 
case 3, 


sys = mdlOutputs(t, x,u); 
case (2, 4, 9} 


sys = []; 
otherwise 

error(['Unhandled flag = ',num2str(flag)]); 
end 


function [sys, x0, str, ts] = ndlInitializeSizes 
sizes = simsizes; 
sizes.NumContStates 
sizes.NumDiscStates 
sizes.NumOutputs = 4; 
sizes.NumInputs - 2; 
sizes.DirFeedthrough 
sizes.NumSampleTimes 


uon 
O A 
ss Ss 


Mo 
o o 
So 


sys = simsizes(sizes); 

x0 - [0;0;0;0]; 

str-[]; 

ts-[]; 

function sys = mdlDerivatives(t, x,u) 
qi - x(1);da1 - x(2); 

q2 = x(3);da2 = x(4); 

dq= [dql;dq2]; 


* The model is given by Slotine and Weiping Li(MIT 1987) 
alfa-6.7;beta- 3.4; 
epc = 3. 0;eta = 0; 


w= Aa 

lc1 = 1/2; I1 = 1/12; 

g=9.8; 
el-ml*11*]lcl- I1- m1 * 11^2; 
e2 = g/11; 


M= [alfa + 2 * epc * cos(q2) * 2 * eta * sin(q2), beta + epc * cos(q2) + eta * sin(q2); 
beta * epc * cos(q2) * eta * sin(q2), beta]; 

C-[(-2* epc * sin(q2) * 2 * eta * cos(q2)) * da2, ( - epc * sin(q2) + eta * cos(q2)) * dq2; 
(epc * sin(q2) - eta * cos(q2)) * dq1,0]; 

G= [epc * e2 * cos(q1 + q2) + eta * e2 * sin(ql + q2) + (alfa- beta + el) * e2 * cos(q1); 
epc * e2 * cos(q1 + q2) + eta * e2 * sin(ql * q2)]; 


tol(1) =u(1); 
tol(2) = u(2); 


176 «(|| 机 器 人 控制 系统 的 设计 与 MATLAB 仿 真 ， 基本 设计 方法 (第 2 版 ) 


ddq= inv(M) * (tol'- C* dq- G); 
sys(1) = x(2); 

sys(2) = ddq(1); 

sys(3) = x(4); 

sys(4) = ddq(2) ; 

function sys = mdlOutputs(t, x,u) 
sys(1) = x(1); 

sys(2) = x(2); 

sys(3) = x(3); 

sys(4) = x(4); 


(4) 作 图 子 程序 : chap? 1plot. m, 
close all; 


figure(1); 

subplot(211); 

plot(t, yl(:,1), 'r', t, y1(:, 2) , 'b', 'linewidth',2); 
xlabel('time(s)');ylabel('angle tracking of joint l'); 
subplot(212); 

plot(t, y2(:,1), 'r', t, y2(:,2), 'b', 'linewidth',2); 
xlabel('time(s)');ylabel('angle tracking of joint 2'); 


figure(2); 

subplot(211); 

plot(t, - pi* sin(pi*t),'r',t,y1(:,3), 'b', 'linewidth',2); 
xlabel('time(s)');ylabel('angle speed tracking of joint 1'); 
subplot(212); 

plot(t,pi* cos(pi* t), 'r', t, y2(:,3), 'b', 'linewidth',2); 
xlabel('time(s)');ylabel('angle speed tracking of joint 2'); 


figure(3); 

subplot(211); 

plot(t,tol(:,1),'r', 'linewidth',2); 
xlabel('time(s)');ylabel('control input 1"); 
subplot(212); 
plot(t,tol(:,2), 'r', linewidth',2); 
xlabel('time(s)');ylabel('control input 2"); 


7.3 基于 输入 输出 稳定 性 理论 的 滑 模 控制 


7.3.1 系统 描述 
机 械 手 n 关节 机 械 手 的 动态 模型 为 
M(g)g+C(g,g)qg +G(g)=r (7. 13) 

其 中 ,M (gq ) 为 正定 质量 惯性 矩阵 ,C(qg,d ) 为 哥 氏 力 和 离心 力 项 ,G (gq ) 为 重力 ,r 为 控制 输 
入 入 号 。 

7.3.2 控制 律 的 设计 

设 机 械 手 所 要 完成 的 任务 是 跟踪 时 变 期 望 轨 迹 qi GO ,位 置 跟踪 误差 为 

e —q4—4 
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— 


定义 
d. =G FÅ (qa =q) 
根据 式 (7. 3) 及 附录 式 (3) ,机 械 手 动力 学 系统 具有 如 下 动力 学 特性 : 存在 向 量 p € R", 
满足 
M(q0q, - C(q.q04, - G(qD) —Y(Q.qQ.q,. d.) p 


- nid did (7.14) 
M(q0q, + C(q:-4)q4: + G(g) —Y(q.q.q..q2.) p 
取 滑 模 面 
s$ 一 4 一 0 一 (4 一 和) 十 人 A(gu 一 9 一 6 十 人 Ae (7. 15) 
其 中 ,A 为 正 对 角 和 矩阵 。 
令 Lyapunov 函数 为 
VG) - 8 MG 
则 
VG) 一 STM(Cq)S + gs MGDs —s$'M(DS 4 s'C(QQ.q)s 
—s'[M(qX(, —4) -C(q.q)(4, —4)] 
—s" [M (i, -CXGq3 Dd, --GGp —c] (7.16) 
可 采用 以 下 两 种 方法 实现 滑 模 控制 。 
方法 一 : 基于 估计 模型 的 滑 模 控制 。 
设计 控制 律 为 
r 一 M(qg)g, + CQ.4)04, + GOD +r, (7.17) 


其 中 ,t, 为 待 设计 项 。 
将 式 (7.17) 代 入 式 (7. 16) 得 


VG) —5s'[M Q)4, - C(q 404. - G!D — MD, — €CGQé404, — G(q)—r.] 
—s' (M (q). +C (4.404, + G(q) — c) —s' (Y(q q.d, d) p — c) 


其 中 ， 

P= LD Dr Ds |P; |K a; 

Y(q,d,q d =g] (Ys [se Ys SelB 
则 只 要 选取 


k,sgn(s;) +s; 
t, —ksgn(s) +s = : (7.18) 
k,sgn(s,) ts: 
H'B.k-—[£, sk ET k; = 27 Y*a; $1 2]1,2,*9*5m, 
j=l 
于 是 有 | 
Va) = 22 A )uYSE 一 > sikisgn(s;) 一 pt 
i=l i-1 


i=] j-1 


二 
l 


=i j-l i=] j=1 i=l m 
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方法 二 : 基于 模型 上 界 的 滑 模 控制 。 
式 (7. 16) 可 写 为 


VG) ——s'[r — (Mä. - C(q 404, -G(q)] 2 —5s" [e — YQ d... d) p] 


若 能 估计 出 
p—[pebpecebl. l p; ISË; 
Y(q.d.dod)O —[Y;]. |Y} ISKY, i—1.2,-.n 
将 控制 律 设 计 为 
k,sgn(s,) +s; 
t= ksgn(s) +s 一 : (7.19) 


k,sgn(s,) 十 5， 


其 中 ,二 [Ey kask, Kk, = D Yp; i 1,2, 
;=1 
于 是 有 


V(t) 一 一 [3 sK,sgnGs;) 十 Sa 一 » Sirga] 
i-1 i=l 


S [> 2j ls: | Y55; + 2j5 - 2; 2,5Yybi] «— 3s «o 
由 式 (7.19) 可 知 ,该 控制 律 计 算 量 较 控 制 律 式 (7. 17) 减 少 , 不 需要 在 线 估计 之 值 ,但 需 


要 较 大 的 控制 量 。 由 控制 律 式 (7. 19) 中 切换 项 增益 k, 和 控制 律 式 (7. 17) 中 切换 项 增益 k, 
的 定义 可 知 ,k; ZEE k; 的 值 大 , 故 控制 律 式 (7. 19) 造 成 的 拌 振 比 控制 律 式 (7. 17) 的 大 。 


7.3.3 仿真 实例 
取 双 关节 机 械 臂 作为 被 控 对 象 ,其 动态 方程 取 式 (7. 132 (动力 学 方程 (7. 13) 及 线性 化 推 
导 见 本 章 附 录 ) , 即 
M(q)0q - C(q.q)0q + G(q) =r 
其 中 ,一 [La， q:] ,rz 一 [mn e]. 


取 

Mis a + 2ecos(q;) T-2ysin(q;) P -d-ecos(q;) + ysin(q;) 

nini B + €cos(q;) + ysin(q;) B 

. (— 2esin(q;) + 2ycos(q;))d, (一 ssin(qy ) + gcos(q;))q, 
C(q.q) = 
(esin(q;) — gcos(q;))q, 0 

diras ee; cos(q, q2) + geo5sin(q, d- q?) + Ca — B 4- ejDescos(qi) 

M ee; cos(q, Fq) + ge;sin(q, 十 qs) 


其 中 ,参数 a,B,e,y 分 别 是 机 械 力 臂 方 程 中 未 知 物理 参数 的 函数 ,a = I Hm lia 十 I 十 
mle tm. dli p= tm daem. l ecos.) ,7 二 melilisin(6.), 机 械 臂 的 实际 物理 参数 
值 见 表 7-1。 取 p=[a B e w]'=[6.7 3.4 3.0 O]l'.p—-[é B ê 7$] —0.95p. 
m= rj —1,2,3,4, 
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两 力 辟 机械 手 两 个 关节 的 角度 指令 分 别 为 gu 二 sin(2xt) ,quw 二 sin(2xt)。 滑 模 控制 律 
ama- s. 
0 15 
模型 初始 值 为 [1 0 1 0], 采 用 控制 律 (7.17) , 取 a= lp; | 十 0. 50, EFE IHE F=, 
第 一 关节 和 第 二 关节 的 角度 跟踪 仿真 结果 如 图 7-5 一 图 7-7 所 示 。 同 理 采 用 控制 律 (7. 195. 


取 p, — | p, | 十 0. 50 ,程序 中 取 下 一 2, 同 样 可 以 得 到 第 一 关节 和 第 二 关节 的 角度 及 角速度 跟 
踪 仿 真 结果 。 


x 
K- 
把 1 
2 
u 0 
o 
E 
g - 
tb 
8 2 
0.5 l 1.5 2 2:5 3 
time(s) 
x 
B 
p» 
o 
aD 
[-] 
i 
E 
o 
2 
v 
> 
v 
E 0 0.5 1 1.5 2 2.5 3 
= time(s) 


7-5 第 一 关节 的 角度 及 角速度 跟踪 


PN 


angle tracking of link 2 


l 
一 2 


e 
e 
un 
un 
N 
N 
n 
v3 


z time(s) 

D 

人 

o 

EOS 

Lx 

B o 

E 

$ -5 

E 

o 

C ERU 

8 0.5 1 1.5 2 2.5 3 
time(s) 


7-6 第 二 关节 的 角度 及 角速度 跟踪 
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— 1000 
x 
K=] 
M 
o 
5 0 
a 
B 
E 
€ 
S -1000 
0 0.5 1 1.5 2 2.5 3 
time(s) 
« 1000 
E 
= 
"5 500 
5 
a 
S 0 
£ 
g 
8 -500 
0 0.5 1 1.5 2 2:5 3 
time(s) 


图 7-7 控制 输入 信号 


仿真 程序 如 下 : 
(1) Simulink 主 程序 : chap7_2sim. mdl。 


To Workspace2 


chap7_2ctl 


S-Function4 


To Workspace1 


S-Fundion3 To Workspace3 


(2) 控制 律 子 程序 : chap? 2ctrl. m, 


function [sys,x0, str,ts] = control strategy(t, x, u, flag) 
switch flag, 
case 0, 
[sys, x0, str, ts] = ndlInitializeSizes; 
case 3, 
sys = mdlOutputs(t,x,u); 
case (2,4,9) 
sys-[]; 
otherwise 


error(['Unhandled flag = ',num2str(flag)]); 
end 


function [sys, x0, str, ts] = ndlInitializeSizes 
Sizes - simsizes; 
sizes.NumOutputs 
sizes. NumInputs - 10; 
sizes.DirFeedthrough - 1; 
sizes.NumSampleTimes - 
SyS 7 simsizes(sizes); 
x0 = []; 

str = []; 

ts * []; 


I 
N 
S 


I 
e 
M. 
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function sys = mdlOutputs(t, x, u) 

ql d-u(1);dql d=u(2);ddql d= u(3); 
q2 d-u(4);dq2 d= u(5);ddgq2 d= u(6); 
ql=u(7);dql = u(8); 

q2 = u(9);dq2 = u(10); 

dq= [dql;dq2]; 


p=[6,7 3:4 3:0 0]; % Practical p 
ep= 0.95 * p; % Estimated p 


alfa p= ep(1); 
beta p= ep(2); 
epc p- ep(3); 
eta p= ep(4); 


mi = 1;11=1; 

Tei = 1/2711 = 1/12; 

g=9.8; 
el-nl*11*1lcl-I1-ml1*11^2; 
e2 = g/11; 


dq d= [dai d,dq2 d]'; 
ddq d= [ddql d,ddq2 d]'; 


e-[al d-qi,q2 d- q2]'; 
de = [dgql d- dq1,dq2 d- dq2]'; 


M p= [alfa p*2* epc p* cos(q2) * 2* eta p* sin(q2), beta p* epc p* cos(q2) + eta px sin(q2); 
beta p +t epc p * cos(q2) + eta px sin(q2), beta p]; 

C p*[(-2*epc px sin(q2) * 2* eta px cos(q2)) * dq2, ( - epc px sin(q2) + eta p * cos(q2)) 

* dq2; 
(epc px sin(q2) - eta px cos(q2)) * dql,0]; 

G p7 [epc p* e2 * cos(q1 + q2) + eta p* e2 * sin(ql + q2) + (alfa p- beta p + el) * e2 * cos(q1); 
epc p * e2 * cos(q1 + q2) + eta p * e2 * sin(ql + q2)]; 


Fai- 15 * eye(2); 
s-de*tFai*e; 

dqr = dq_d + Fai * e; 
ddqr = ddq d + Fai * de; 


Y= [ddgqr(1) + e2 * cos(q1),ddqr(2) ~ e2 * cos(q1),2 * cos(q2) * ddar(1) * cos(q2) * ddqr(2) 一 2 关 
sin(q2) * dq2 * dgqr(1) - sin(q2) * dq2 * dqr(2) + e2 * cos(q1 + q2),2 * sin(q2) * ddqr(1) + sin(q2) 
* ddqr(2) * 2 * cos(q2) * dq2 * dqr(1) * cos(q2) * dq2 * dar(2) + e2 * sin(q1 + q2); 
0,ddar(1) + ddar(2),cos(q2) * ddqr(1) + sin(q2) * dql * dqr(1) + e2 * cos(q1 + q2), sin(q2) * 

ddqr(1) - cos(q2) * dql * dqr(1) + e2 * sin(q1 + q2)]; 
Y max- abs(Y) * 0.10; 
F=1; 
if F== 1 

a=abs(p- ep) * 0.50; * Upper p- ep 

k=Y max*a'; 

tols = [sign(s(1)) 0;0 sign(s(2))] *k* s; 

tol =M p* ddqr + C_p * dqr + G_p + tols; 
elseif F == 

p_up= p+ 0.50; % Upper p value 

k up- Y max * p up'; 

tol = [sign(s(1)) 0;0 sign(s(2))] * k up* s; 
end 
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sys(1) = tol(1); 
sys(2) = tol(2); 


(3) 被 控 对 象 子 程序 : chap7_2plant. m, 


function [sys, x0, str,ts] =s_function(t, x,u, flag) 
switch flag, 


case 0, 

[sys, x0, str,ts] = mdlInitializeSizes; 
case 1, 

sys = ndlDerivatives(t,x,u); 
case 3, 


sys = ndlOutputs(t,x,u); 
case (2, 4, 9 ) 

sys = []; 
otherwise 

error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0, str,ts] = ndlInitializeSizes 
Sizes - simsizes; 
sizes.NumContStates 
sizes.NumDiscStates 
sizes.NumOutputs 
sizes.NumInputs 
sizes.DirFeedthrough 
sizes. NumSampleTimes 
Sys = simsizes( sizes); 
x0 7» [1.0,0,1.0,0]; 
str-[]l; 
ts- []; 
function sys = mdlDerivatives(t, x, u) 
tol- [u(1);u(2)]; 
qrtex(1); 
dai = x(2); 
q2- x(3); 
dg2 = x(4); 


Wow 
Ne Se 


[I 
OoOoOot5o 


E 


u 
SY ee ^n 


p=[6.7 3.43.0 0]; 


alfa = p(1); 

beta = p(2); 

epc = p(3); 

eta = p(4); 

ml = 1;11=1; 

yei = 1/2; 71 = 1/12; 

g=9.8; 

el-ml1*1li1*]lcl- I1-ml*]11^2; 
e2 = g/11; 


M= [alfa + 2 * epc * cos(q2) * 2 * eta * sin(q2), beta + epc * cos(q2) + eta * sin(q2); 
beta * epc * cos(q2) * eta * sin(q2), beta]; 

C-[(-2* epc * sin(q2) * 2* eta * cos(q2)) * da2, ( - epc * sin(q2) + eta * cos(q2)) * da2; 
(epc * sin(q2) - eta * cos(q2)) * dq1,0]; 

G= [epc * e2 * cos(q1 + q2) + eta * e2 * sin(ql * q2) + (alfa- beta + el) * e2 * cos(q1); 
epc * e2 * cos(q1 + q2) + eta * e2 * sin(q1 + q2)]; 

* robot dynamic equation as 
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S= inv(M) * (tol ~- C* [dq1;dq2] - G); 


sys(1) = x(2); 
sys(2) = S(1); 
sys(3) = x(4); 
sys(4) = S(2); 
function sys = mdlOutputs(t, x, u) 
sys(1) = x(1); 
sys(2) = x(2); 
sys(3) = x(3); 
sys(4) = x(4); 


(D 输入 指令 子 程序 : chap? 2input. m, 


function [sys,x0, str,ts] = input(t,x,u, flag) 
switch flag, 
case 0, 

[sys, x0, str, ts] = ndlInitializeSizes; 
case 3, 


sys = mdlOutputs(t, x, u); 
case {2,4,9} 

sys-[]; 
otherwise 

error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0, str, ts] = ndlInitializeSizes 
Sizes = simsizes; 
sizes.NumOutputs = 6; 
sizes.NumInputs = 0; 
sizes.DirFeedthrough 
sizes.NumSampleTimes 
sys = simsizes(sizes); 


0; 
0; 


x0 - []; 
str = [D 
ts = [p 


function sys = mdlOutputs(t, x, u) 

ql d= sin(2 * pi* t); 

q2_d= sin(2 * pi * t); 

dql_d= 2 * pi * cos(2 * pi * t); 
dq2_d= 2 * pi * cos(2 * pi * t); 

ddql d= - (2 * pi)^2* sin(2 x pi*t); 
ddq2 d= — (2 * pi)^2 * sin(2 * pi * t); 


sys(1)7 ql d; 
sys(2) = dal d; 
Sys(3) = ddql d; 
sys(4) = q2 d; 
sys(5) = dq2 d; 
sys(6) = ddq2 d; 


(5) 绘图 子 程序 : chap? 2plot. m. 


close all; 


figure(1); 
subplot(211); 
plot(t,qd(:,1), 'r',t,q(:, 1), 'b', 'linewidth',2); 
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xlabel('time(s)');ylabel('angle tracking of link 1'); 
subplot(212); 

plot(t,qd(:,2), 'r',t,q(:,2), 'b', 'linewidth',2); 
xlabel('time(s)');ylabel('angle velocity tracking of link 1'); 


figure(2); 

subplot(211); 

plot(t,qd(:,4), 'r',t,q(:,3), 'b', 'linewidth',2); 
xlabel('time(s)');ylabel('angle tracking of link 2'); 
subplot(212); 

plot(t,qd(:,5), 'r',t,q(:,4), 'b', 'linewidth',2); 
xlabel('time(s)');ylabel('angle velocity tracking of link 2'); 


figure(3); 

subplot(211); 

plot(t,ut(:,1), 'r','linewidth',2); 
xlabel('time(s)');ylabel('control input of link 1'); 
subplot(212); 

plot(t,ut(:,2), 'r', 'linewidth',2); 
xlabel('time(s)');ylabel('control input of link 2'); 


7.4 基于 LMI 的 指数 收敛 非 线 性 干扰 观测 器 的 控制 


7.4.1 非 线 性 干扰 观测 器 的 问题 描述 
考虑 双关 节 机 械 手 动力 学 方程 : 
J(808--C(0 ,0) 0+G(0)=r+d (7. 20) 
其 中 ,J (GO ER” 为 机 械 手 的 惯性 矩阵 ,C(8 ,0 )ER: 表示 离心 力 和 哥 氏 力 项 ,G(0 )ER 为 
重力 项 ,ER ,6 € R' fle ER 分 别 代表 角度 .角速度 和 控制 输入 ,daER: 为 外 界 干扰 。 
文献 [2] 中 所 设计 的 非 线 性 干扰 观测 器 的 不 足 之 处 : 要 求 模 型 中 的 惯性 矩阵 J O ) 必 须 


i ql. 1 O H " 
满足 特殊 形式 , 即 (9) = | TE 1 "其 中 ,J (9) 必须 满足 了 (9) = 


jui—2jsttii Jaja tX cosl) = ad 
j i [MIOPE ANTENNA RAT 
2—j3 t X,cos(0;) Ja 
该 观测 器 在 其 他 类 模型 的 应 用 。 


为 了 克服 上 述 缺 陷 , 文 献 L3] 提 出 了 一 种 改进 的 设计 算法 ,本 节 讨 论 采用 该 方法 设计 机 
械 手 非 线性 干扰 观测 器 及 相应 的 控制 器 的 设计 方法 。 


7.4.2 非 线性 干扰 观测 器 的 设计 
非 线性 干扰 观测 器 设计 为 


i —L(8)(C(Ó 00 二 GO ) 一 rr) 一 LO)d 
. (7. 21) 
d =z + pÅ) 


为 了 克服 观测 器 的 不 足 之 处 ,文献 L3] 中 取 
Lb(05—xJ3 (85 (7. 22) 
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Tivi 


p(àb-—x'ó (7. 23) 
其 中 ,X A np ug p ,通过 线性 矩阵 不 等 式 来 求 。 
令 


p —LGO»JIO»8 
式 (7. 21) 式 (7. 22) 和 式 (7. 23) 构 成 了 非 线 性 干扰 观测 器 。 一 般 没 有 干扰 d 的 微分 的 先 验 
知识 ,假设 相对 于 观测 器 的 动态 特性 干扰 的 变化 是 缓慢 的 四, 则 可 取 d —0, 
设计 Lyapunov PR Zt Jy 
V, —d'X'J(0)Xd 
RB,J(0»-J(8»)'»0, 
于 是 
V, -arTXTJ(O)XdE Hd X^ j 60Xd +å"X"J (0 )Xd 
根据 观测 器 式 (7.21) ,可 得 


d=å— d =d — —p®) 


—d —L(8»(C(8 ,00 8 --G(8) — c) - L(ODd — L(O Oo 
—d +L d —L(G)(0(9)8-4-c« ,00--GXO ) v) 
=å - L(8)d — Ne 


=d —L(80d 
因而 得 到 观测 误差 方程 为 
d --L(8)d —0 -— 
从 而 得 到 
4 一 一 L(0)4 ——X^J^ (00d 
d! -—(QJ^ (004) ——J7 (00X7 
则 


V, —d'X'J (0 )Xd Hd X" j 60Xd TaTXTJ(O »Xd 
—-—4d'J  (00X7X'J(00Xd td Xj 60Xd —d'X' J O0 XX^ J^ (00d 
— —d' Xd -d'X'j (0 )Xd —d'X'd 
—-—d'(X —X'j (05x 4- Xd 


构造 如 下 不 等 式 : 
X-X'—X'jaox2r (7. 25) 
其 中 ,了 0 为 对 称 正定 阵 。 
于 是 
V, <—d™Td 


可 见 ,干扰 观测 器 指数 收敛 ,收敛 精度 取决 于 参数 T 值 ,TT 值 越 大 ,收敛 速度 越 快 , 精 
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度 越 高 。 


7.4.3 LMI 不 等 式 的 求解 


由 不 等 式 (7. 25) 可 见 , 式 中 含有 非 线 性 项 ,必须 转换 为 线性 矩阵 不 等 式 才 能 求解 。 令 

Y-X ', KY =(X 5' 和 Y= 二 XX 分 别 乘 以 式 (7. 25) 的 左右 两 边 ,得 

YIT 十 Y 一 0 ) 三 YIPY 
即 

YT -Y -Y'TY 2j(6) 
由 于 1 J (01 se. J (os er y Est sz d 354 2 (EO 

Y' -Y-Y'rYztl 

Bil 

Y'-Y—tr —Y'rY z0 


根据 Schur 补 定理 上 假设 C 为 正定 矩阵 , 则 A — BC BT 0 等 价 为 a 2 dii 


WEREMA 
Y'+Y—¢I Y! 
D m 
通过 MATLAB 下 的 LMI 工具 箱 ( 建 议 采 用 新 的 LMI 求解 工具 箱 一 一 YALMIP 工具 
箱 ) 求 解 式 (7.26), 便 可 求 得 Y, 从 而 得 到 XX。 该 不 等 式 的 求解 是 否 有 效 取 决 于 《和 械 值 。 
5 越 小 .了 越 小 , 越 容易 得 到 有 效 的 解 。 


7.4.4 计算 力矩 法 的 滑 模 控制 


采用 观测 器 式 (7. 21) 观 测 干扰 d ,在 滑 模 控制 中 对 干扰 进行 补偿 ,可 有 效 地 降低 切换 增 
益 , 从 而 有 效 地 降低 抖 振 。 
关节 的 理想 角度 为 0, , 取 跟 踪 误 差 e 二 9 一 04 ,定义 滑 模 函数 为 


|>。 (7. 26) 


s—é--Ae C7.27) 
Al 0 
其 中 ,A = T 
0 Àz 
Td 
$—20—8,--Aé—J ^" (—CÓ—G--d) —8,--Aé 
Wife asy 


c—J v 4- CÓ -G — qsgns —d — Cs (7. 28) 
„u 9 3 PEN 
其 中 ,7 = 0 q sh: > d C | 4-94 534470. 1,2, 
2 


从 而 有 
Jš =J v — gsgns —d +d — Cs +J (A é —6,) =J (v +A é —0,) — gsgns +d — Cs 
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其 中 ,d= 二 4 一 d, 取 
v=04—Aé (7. 29) 
则 Js 二 一 ysgns 十 d 一 Cs ,由 于 J(9 ) 为 正定 阵 , 设 计 闭环 系 统 Lyapunov 函数 为 
V-7sJs +V, 
AFPR ROS M d s ao I. 1g 109 ld Go D WA 
Vs + sts +V,=s'(—Cs— nsgns HD 3s js 十 V。 
=—7 lis || -s1 +s" 一 2C)s +V, 
=—y lls || —s*d V, <- s ls || —d"rd 


当 V=0 时 ,s 二 0,d 王 0, 根据 LaSalle 不 变性 原理 ,闭环 系统 为 渐进 稳定 , 当 :一 ce 时 ,s* 一 0， 
d-~~0。 系 统 的 收敛 速度 取决 于 7 DP 。 


7.4.5 仿真 实例 


仿真 实例 1: 干扰 观测 器 开 环 测 试 。 
考虑 稳定 的 SISO 系统 ,模型 为 


à =— 250 -- 133(c +d) 


Xl te J (008--CKÓ ,6)6 +G) —c 十 d ,可 知 fant. rin 不 等 式 (7. 25) E X+ 


133 133* 
X'2D , 取 XX=T 二 1, 可 满足 该 不 等 式 的 要 求 。 
分 别 取 d(x) 二 一 5 和 qd) 一 0.05sint 。 干 扰 观测 器 采用 式 (7.21) 一 式 (7. 23), 仿 真 结 
果 如 图 7-8 和 图 7-9 所 示 。 


0 
8. nin 
8 d estiamtion 
Ea 
$ 
2-3 
v 
S 4 
v 

9 1 2 3 4 5 
time(s) 


error between d and its estimation 
| 
I] 


0 1 2 3 4 5 
time(s) 
7-8 d(1))7 —5 的 干扰 观测 及 观测 误差 
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0.05 


— d 


d estiamtion 


d and its estimantion 
o 


time(s) 


N 


l 
N 


error between d and its estimation 
e 


time(s) 


E 7-9 d(1)-0.05sinr 的 干扰 观测 及 观测 误差 


仿真 程序 如 下 : 
(1) Simulink 主 程序 : chap7_3sim. mdl。 


chap7_3plant 


Sine Wave To Workspace2 


Clock To Workspace S-Function2 To Workspace1 


Mux1 


(2) 被 控 对 象 程序 : chap7_3plant. m, 


function [sys,x0, str,ts] = NDO plant (t,x,u, flag) 
switch flag, 
case 0, 
[sys, x0, str, ts] = ndlInitializeSizes; 
case 1, 
Sys = ndlDerivatives(t,x,u); 
case 3, 
sys = ndlOutputs(t, x, u); 
case (2, 4, 9] 
sys = []; 
otherwise 
error(['Unhandled flag = ',num2str(flag)]); 
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end 
function [sys, x0, str, ts] = mdlInitializeSizes 
sizes = simsizes; 


LU 


sizes.NumContStates 2 
sizes.NumDiscStates  - 0 
sizes.NumOutputs = 3; 
sizes.NumInputs = 工 
sizes.DirFeedthrough = 1 
sizes.NumSampleTimes = 0 
sys = simsizes(sizes); 
x0=[0.1,0]; 

str-[]; 

ts= []; 

function sys = mdlDerivatives(t, x,u) 
ut-u(1); 

*&dt-2-5; 

dt = 0.05 * sin(t); 

sys(1) = x(2); 

sys(2) = -25 * x(2) + 133 * (ut * dt); 
function sys = mdlOutputs(t, x, u) 
*&dt-2-5; 

dt- 0.05 * sin(t); 

sys(1) = x(1); 

sys(2) = x(2); 

sys(3) = dt; 


(3) 于 扰 观测 器 程序 : chap? 3obv. m, 


function [sys, x0, str,ts] = NDO(t,x,u,flag) 
switch flag, 
case 0, 

[ sys, x0, str, ts] = ndlInitializeSizes; 
case 1, 

sys = ndlDerivatives(t,x,u); 
case 3, 

sys = ndlOutputs(t, x, u); 
case (2, 4, 9 ) 

sys = []; 
otherwise 

error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0, str, ts] = ndlInitializeSizes 
sizes - simsizes; 
sizes.NumContStates 
sizes.NumDiscStates 
sizes.NumOutputs 
sizes.NumInputs 
sizes.DirFeedthrough 
sizes.NumSampleTimes 


L LU 
OPAPOP 


sys = simsizes(sizes); 


x0= [0]; 
str= [ 15 
ts-[]; 


function sys » ndlDerivatives(t,x,u) 
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J = 1/133;C = 25/133;G = 0; 


tol-u(1); 
dth= u(3); 
z= x(1); 


X=1; 

L= inv(X) * inv(J); 
p= inv(X) * dth; 
d=z+p; 


dz-L*(C*dth*G-tol)-L*d; 
sys(1) = dz; 

function sys = mdlOutputs(t, x, u) 
dth= u(3); 

z-5(1); 


X21; 
p= inv(X) * dth; 
d-z*p; 


sys(1) =d; 
(4) 作 图 程序 : chap? 3plot. m, 


close all; 


figure(1); 

subplot(211); 

plot(t,d(:,3), 'r',t,dp(:,1),'- .b', 'linewidth',2); 
xlabel('time(s)');ylabel('d and its estimation'); 

legend( 'd', 'd estiamtion'); 

subplot(212); 

plot(t,d(:,3) - dp(:,1), 'r', 'linewidth',2); 
xlabel('time(s)');ylabel('error between d and its estimation'); 


仿真 实例 2: 基于 干扰 观测 器 补偿 的 滑 模 控制 。 
二 关节 机 械 手 动力 学 方程 为 


J(8»08--G(6) —c--d 
ji-2X,cos(0;)  j;d- X,cos(0;) 
merae E ja 
= ls = —0. 01, 
RERI A dÅ) =k k, =0. 20,k, 二 0.20。 关 节 一 和 关节 二 的 理想 轨迹 分 别 为 
04, —0. 1sint MOa —0. lsint。 
干扰 观测 器 采用 式 (7. 210 —3X C7. 23) ,该 观测 需 不 需要 加 速度 信号 FA d 的 观测 初始 值 


0. Olgcos(0, -F0,) 
0. 0lgcos(0 3-0, ) f 


EC Aes | 


; —2X,sin(0,) +Ê; —X,sin(6,) * 0, : 
取 [0,0]。 ario] E ILJ «8» || c. nT HE 


— X,sin(0, ) : B, 0 


t—3.0, 考虑 两 个 关节 的 动态 特性 不 同 , XR P = b. 0 


à MEI 26), 可 得 
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o 


-| 0 
Jo 0. 3503 
采用 控制 器 式 (7. 280 和 式 (7. 290 ,被 控 对 象 初始 状态 为 [0.1 0 0.1 0], WA = 


Ds 


l0 0 0.10 0 
» o 1 = b á si OK HI BUR PR RR ER E BE PROC. BU VIZ ERE Dg A — 0. 20, 


仿真 结果 如 图 7-10 一 图 7-13 所 示 。 


angle tracking of first link 
b o o o 
Ss o 一 N uo 
4 


0 1 2 3 4 5 6 7 8 9 10 

time(s) 

2x 

B ol n 

v 

一 

3 as 

S p 

Q 

m p 

5S 

x 

$ -0.05 

2 

S -0.1 

s ] 2 3 4 9. 6 7 8 9 10 
time(s) 


图 7-310. 关节 一 和 关节 二 角度 跟踪 


angle speed tracking of second link angle speed tracking of first link 


time(s) 


图 7-11 关节 一 和 关节 二 角速度 跟踪 
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dtl and its estimation 


dt2 and its estimation 


toll 


tol2 


仿真 程序 如 
(1) LMI 求 


clear all; 
close all; 
Y= sdpvar(2, 
Kesi = 3; 
Gama = 0.10 * 


0.1 
0.05 
0 
—0.05 
-0.1 
0 1 2 3 4 5 6 y 8 9 10 
time(s) 
0.2 
0.1 
0 
-0.1 
-0.2 
2 3 4 5 6 7 8 9 10 
time(s) 
7-12 关节 一 和 关节 二 的 干扰 观测 结果 
1 
0.5 
0 
-0.5 
一 
0 1 2 3 4 5 6 7 8 9 10 
time(s) 
0.5 
0 
-0.5 
-l 
0 1 2 3 4 5 6 7 8 9 10 
time(s) 
7-13 关节 一 和 关节 二 上 的 控制 输入 
FP: 
解 程序 : Imi X. m, 


2); 


[1 0;0 3]; 
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FAI-[Y-*Y'- Kesi * eye(2) Y';Y inv(Gama)]; 
* LMI description 

L1 = set(Y» 0); 

L2 = set(FAI» 0); 


L= L1 + L2; 
solvesdp(L); 
Y = double(Y); 
X= inv(Y) 


(2) Simulink 主 程序 : chap? 4sim. mdl。 


To WorkspaceS 


chap?7. 4input 


S-Function3 


To Workspace2 


Clock To Workspace 


S-Function2 


(30 控制 器 程序 : chap? 4ctrl. m, 


function [sys, x0, str,ts] = s function(t, x,u, flag) 
switch flag, 
case 0, 
[sys, x0, str, ts] = ndlInitializeSizes; 
case 3, 
sys = mdlOutputs(t,x,u); 
case (2, 4, 9 } 
sys = []; 
otherwise 
error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0, str, ts] = mdlInitializeSizes 


Sizes - simsizes; 


sizes.NumContStates = 0; 
sizes.NumDiscStates = 0; 
sizes. NumOutputs = 2; 
sizes.NumInputs - 10; 
sizes.DirFeedthrough - 1; 
sizes.NumSampleTimes - 0; 


sys = simsizes(sizes); 
x0-[]; 
str-[]; 
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ts=[]; 

function sys = mdlOutputs(t,x,u) 
thld= 0.1* sin(t); 

dthid = 0.1 * cos(t); 

ddthid- - 0.1 * sin(t); 


th2d= 0.1 * sin(t); 
dth2d= 0.1 * cos(t); 
ddth2d- - 0.1 * sin(t); 


thd = [thld th2d]'; 
dthd = [dthld dth2d]'; 
ddthd = [ddthld ddth2d]'; 


thl = u(5);dth1 = u(6); 
th2 = u(7);dth2 = u(8); 
dp= [u(9) u(10)]'; 


th= [th1 th2]'; 
dth= [dth1 dth2]'; 


e=th-thd; 
de = dth - dthd; 


Fai = 10 * eye(2); 
s-de*Fai*e; 


g79.8; 

jl1=0.1;j2= 0;j3=0.01;Xp= 0.01; 

J = [j1 +2 * Xp* cos(th2) 32 + Xp * cos(th2) 
j2 + Xp * cos(th2) j3]; 

C=0; 

Gl=0.01#*gxcos(thl + th2); 

G2 = 0. 01 * g * cos(th1 + th2); 

G= [G1;G2]; 


Xite = 0.10 * eye(2); 
% Saturated function 
delta = 0. 20; 
kk = 1/delta; 
fori-1:2 
if abs(s(i))» delta 
sats(i) = sign(s(i)); 
else 
sats(i) = kk* s(i); 
end 
end 
v 7 ddthd - Fai * de; 
tol-J*v*tC*dth*G- Xite* [sats(1) sats(2)]'- dp- C* s; 


sys(1) = tol(1); 
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sys(2) = to1(2); 
(4) 被 控 对 象 程序 : chap7_4plant. m, 


function [sys,x0, str,ts] = NDO plant (t,x, u, flag) 
switch flag, 
case 0, 
[sys, x0, str, ts] = ndlInitializeSizes; 
case 1, 
sys = ndlDerivatives(t,x,u); 
case 3, 
sys = ndlOutputs(t, x, u); 
case (2, 4, 9 ) 


sys = []; 
otherwise 
error(['Unhandled flag = ',num2str(flag)]); 


end 
function [ sys, x0, str, ts] = ndlInitializeSizes 
global k1 k2 

k1 = 0.2;k2 = 0.2; 

sizes = simsizes; 
sizes. NumContStates = 
sizes. NumDiscStates = 
sizes. NumOutputs = 
sizes.NumInputs - 
sizes.DirFeedthrough - 


O rnm NO OO 心 
x 


sizes.NumSampleTimes - 
Sys = sinsizes(sizes); 
x0 - [0.1,0,0.1,0]; 
str-[]; 

ts-[]; 

function sys = mdlDerivatives(t, x,u) 
global kl k2 

tol- [u(1);u(2)]; 

g-79.8; 

j1 = 0.1;j2 = 0;j3 = 0. 01; 

Xp= 0.01; 


J= [j1 +2 * Xp* cos(x(3)) j2 + Xp* cos(x(3)) 
j2 + Xp * cos(x(3)) j3]; 


G1 =0.01 * gx cos(x(1) * x(3)); 
G2 = 0.01 * g * cos(x(1) + x(3)); 
G-[61;G2]; 


dt = [k1 * x(2);k2 * x(4)]; 


S= inv(J) * (tol + dt- G); 
sys(1) = x(2); 
sys(2) = S(1); 
sys(3) = x(4); 
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sys(4) = S(2); 

function sys = mdlOutputs(t,x,u) 
global k1 k2 

dt = [k1 * x(2);k2 * x(4)]; 


sys(1) = x(1); 
sys(2) = x(2); 
sys(3) = x(3); 
sys(4) = x(4); 
sys(5) = dt(1); 
sys(6) = dt(2); 


(5) 干扰 观测 器 程序 : chap? 4obv. m. 


function [sys,x0, str,ts] = NDO(t, x,u, flag) 
switch flag, 
case 0, 

[sys, x0, str, ts] = ndlInitializeSizes; 
case 1, 

sys = ndlDerivatives(t,x,u); 
case 3, 

sys = ndlOutputs(t,x,u); 
case (2, 4, 9} 


sys = []; 
otherwise 
error(['Unhandled flag = ',num2str(flag)]); 


end 
function [sys, x0, str,ts] = ndlInitializeSizes 
sizes = simsizes; 


sizes.NumContStates 


sizes.NumDiscStates 
sizes.NumOutputs 


sizes.NumInputs 


1i 
SS ere 0 t O I 
M 


sizes.DirFeedthrough 


sizes.NumSampleTimes 
sys = simsizes( sizes); 
x0 - [0 0]; 

str*[]; 

ts-[] 

function sys = ndlDerivatives(t, x,u) 
tol- [u(1);u(2)]; 

th- [u(3);u(5)]; 

dth - [u(4);u(6)]; 


g79.8; 

j120.1;j2-70;j3-20.01;X- 0.01; 

J-7[j1*2*X* cos(th(2)) j2* X* cos(th(2)) 
j2*X*cos(th(2)) j3]; 

G1 = 0.01 * g* cos(th(1) * th(2)); 

G2 = 0.01 * g * cos(th(1) + th(2)); 

G= [G1;G2]; 
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X= [0.2837 0; 
0 0.3503]; 


z= [x(1) x(2)]'; 
L= inv(X) * inv(J); 
p= inv(X) * dth; 
dp=z+p; 


dz = L* (G- tol- dp); 

sys(1) = dz(1); 

sys(2) = dz(2); 

function sys = mdlOutputs(t, x, u) 
tol- [u(1);u(2)]; 

th = [u(3);u(5)]; 

dth- [u(4);u(6)]; 


g79.8; 

j120.1;j220;j320.01;X72 0.01; 

J-2[j1*2*X* cos(th(2)) j32*X* cos(th(2)) 
j2*X* cos(th(2)) j3]; 

G1-0.01*g* cos(th(1) * th(2)); 

G2-0.01*g* cos(th(1) + th(2)); 

6-[G1;G2]; 


X = [0.2837 0; 
0 0.3503]; 


z= [x(1) x(2)]'; 
L= inv(X) * inv(J); 
p= inv(X) * dth; 
dp-zt*p; 


sys(1) = dp(1); 
sys(2) = dp(2); 


(6) 作 图 程序 : chap? 4plot. m, 


close all; 

figure(1); 

subplot(211); 

plot(t,thd(:,1), 'r',t,x(:, 1), 'b', 'linewidth',2); 

xlabel( 'time(s)');ylabel('angle tracking of first link'); 
subplot(212); 

plot(t, thd(:,3), 'r', t, x( :, 3), 'b', 'linewidth',2); 
xlabel('time(s)');ylabel('angle tracking of second link'); 


figure(2); 

subplot(211); 

plot(t,thd(:,2), 'r',t,x(:,2), 'b', 'linewidth',2); 
xlabel('time(s)');ylabel('angle speed tracking of first link'); 
subplot(212); 
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plot(t,thd(:,4), 'r',t,x(:,4), 'b', 'linewidth',2); 
xlabel('time(s)');ylabel('angle speed tracking of second link'); 


figure(3); 

subplot(211); 

plot(t,d(:,1), 'r',t,dp(:,1), 'b', 'linewidth',2); 
xlabel('time(s)');ylabel('dtl and its estimation'); 
subplot(212); 
plot(t,d(:s,2), 'x',t,dp(:, 2), b" 'Tinewidth', 2); 
xlabel('time(s)');ylabel('dt2 and its estimation'); 


figure(4); 

subplot(211); 
plot(t(:,1),tol(:,1), 'r', 'linewidth',2); 
xlabel('time(s)');ylabel('toll'); 
subplot(212); 
plot(t(:,1),tol(:,2), 'r', 'linewidth',2); 
xlabel('time(s)');ylabel('tol2'); 


7.5 欠 驱 动 两 杆 机 械 璧 Pendubot 滑 模 控制 


7.5.1 Pendubot 控制 问题 


欠 驱 动 旋 臂 式 两 杆 倒立 摆 机 械 臂 Pendubot 作为 一 类 典型 的 欠 驱 动机 械 系统 ,受到 越 来 
越 多 的 学 者 和 研究 人 员 的 关注 。 欠 驱动 机 械 系统 指 系 统 的 独立 控制 输入 个 数 小 于 系统 自由 
度 个 数 的 非 线 性 系统 。Pendubot 是 一 个 带 有 两 个 旋转 关节 的 两 杆 欠 驱动 机 械 臂 , 它 在 一 个 
垂直 平面 内 运动 ,其 肩 关 节 是 驱动 的 ,而 肘 关节 是 非 驱 动 的 。 和 欠 驱动 机 械 系统 因为 在 节约 能 
量 降低 造价 ,减轻 重量 .增强 系统 灵活 度 等 方面 都 较 传统 的 完全 驱动 机 械 系 统 具 有 优势 ,使 
其 在 许多 重要 领域 都 有 广泛 的 应 用 ,如 空间 机 器 人 \ 水 下 机 器 人 、 多 足 机 器 人 、 移 动机 器 人 、 
柔性 关节 机 器 人 、 体 操 机 器 人 等 。 

目前 ,针对 Pendubot 系统 有 许多 控制 方法 。 线 性 二 次 型 调节 器 (LQR) 作 为 一 种 平衡 
控制 策略 ,最早 被 用 于 Pendubot 系统 ” ,但 对 于 复杂 的 非 线 性 、 强 耦合 系统 ,其 抗 干扰 性 和 
鲁 棒 性 较 差 。 文 献 L6] 根 据 Pendubot 系统 的 无 源 性 设计 了 基于 能 量 的 起 摆 控 制 方法 。 文 献 
[7 提出 了 基于 反馈 镇 定 的 倒立 平衡 点 附近 的 混杂 控制 方法 。 文 献 L8] 应 用 线性 调节 理论 和 
T-S 模糊 实现 了 对 Pendubot 的 跟踪 控制 。 文 献 L9] 提 出 了 一 种 基于 脉冲 动量 的 起 摆 控 制 
策略 。 


7.5.2 Pendubot 机 械 句 建 模 


从 驱 动 倒立 摆 机 械 臂 Pendubot 是 一 个 两 杆 机 械 臂 , 它 带 有 两 个 能 够 360 度 旋转 的 关 
节 ,在 一 个 垂直 平面 内 运动 ,由 电机 驱动 的 关节 称 为 主动 关节 或 者 驱动 关节 ,无 电机 驱动 的 
关节 称 为 从 驱动 关节 或 者 被 动 关节 ,和 主动 关节 相连 的 机 械 臂 称 为 主动 臂 , 和 从 驱动 关节 相 
连 的 机 械 臂 称 为 从 驱动 臂 , 由 于 其 系统 结构 与 人 类 的 手臂 有 些 相似 , 所 以 其 主动 关节 也 经 常 
被 称 为 肩 关 节 , 其 欠 驱 动 关节 被 称 为 肘 关 节 。 
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m 


主动 关节 处 装 有 一 个 直流 电机 ,驱动 整个 机 械 辟 系统 的 运动 。 欠 驱动 机 械 辟 系统 中 主 
动 关 节 和 欠 驱 动 关 节 处 还 安装 了 两 个 高 精度 光电 编码 器 ,分 别 用 于 提供 主动 臂 和 欠 驱 动 辟 
的 位 置 反馈 信号 。 

如 图 7-14 所 示 ,在 与 水 平面 垂直 的 竖 直 平面 上 ,建立 以 主动 关节 中 心 点 为 原点 、 以 水 平 
线 为 模 轴 x 轴 、 以 竖 直 线 为 纵 轴 > 轴 的 zOy 直角 坐标 系 , 设 gq, 为 主动 臂 相对 于 水 平 坐标 轴 
x 的 角度 ,gq; 为 欠 驱 动 臂 相对 于 主动 臂 的 角度 ,/, AEDE RKE, La 为 主动 臂 质心 到 距离 ， 
lo 为 欠 驱 动 辟 质 心 到 主动 关节 的 距离 ,1 为 主动 臂 质心 相对 于 主动 关节 的 转动 惯量 ,1, 为 
欠 驱 动 臂 质心 相对 于 主动 关节 转动 惯量 ,m, 为 主动 臂 质量 ,ms 为 欠 驱 动 臂 质 量 ,g 为 重力 
加 速度 ,rt 是 电机 驱动 力矩 。 


7-14 Pendubot 机 械 臂 示意 图 


首先 考虑 一 般 形 式 的 拉 格 朗 日 - 欧 拉 运动 方程 : 
d [ƏL] oL NETE 
ba i =1,2, n (7. 30) 
其 中 ,L A bHRBEBHTSET.L—K-—P.K 为 机 械 辟 的 总 动能 ,P 为 机 械 臂 的 总 势能 ,q, 为 机 
械 臂 的 广义 坐标 ,r; 为 在 关节 i 处 作用 于 系统 以 驱动 杆 件 的 广义 力 或 广义 力矩 。 
定义 


(7. 31) 
根据 式 (7. 30) 可 得 
da aL, 
digg, ?q — 
"e x (7.32) 
Bag a 
其 中 ,r: 王 0。 
拉 格 朗 日 函数 为 


L(q.d) =K —P (7.33) 
第 一 步 ,计算 Pendubot 主动 臂 的 平移 动能 K, 和 势能 P, 


l 2 ; 
K, =Fmilagi, P|=miluag singi 


第 二 步 ,计算 Pendubot 欠 驱 动 臂 的 平移 动能 K, 和 势能 P,。 根 据 Pendubot 动力 学 模 
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型 示意 图 ,得 到 欠 驱 动 臂 质心 的 笛 卡 儿 坐 标 系 为 


Xs -—l,cos(qi) -- La cos(q, +q) 
(7. 34) 
y? —l,sin(qi) T Lgsin(q, c-q;) 
从 驱动 臂 速度 的 笛 卡 儿 坐 标 分 量 可 以 表示 为 
I. — — [,sin(q1)q, — Lasin(q; 十 qz)(d， 十 dz ) 
(7. 35) 
Ya — [,cos(q1)q, -F la cos(q, +q) (gi cq) 
欠 驱 动 臂 速度 的 平方 值 
vi =i; +5: =liqi T S Cga Tg + 2l,lacos([q?)d, (qi 十 q，) 
则 欠 驱 动 臂 的 平移 动能 为 
K, = mv} -n.tii T s d T9, + 20,0 cos(q5)4 (qı 十 gs )) 
欠 驱 动 臂 的 势能 为 


P,-—ms;gy;-m;g(,sin(qi) d- Lo sin(q, +q:)) 
第 三 步 , 欠 驱动 臂 的 转动 动能 为 


M 1 0 1 1 1 Ii +I; H] [di 
K, = | th | | 
24 I | 2 1 1 q 2 [d d] I, 元 bs 


1 " bx , 
=7 T G)gi -c- 21:910: T gi) 
根据 拉 格 朗 日 函数 可 以 得 到 
L(gq:qg)=K— P=Ki+K;+K,—Pi—P, 
il r 1 23 , " £ ovd s 
=zmlagi tmi T 4S 4.) t 2l; cos(q,)qi (qı +ga) ] + 


1 zs $3 i$ : 
zh + I2q1 - 2150105 + 1:02) — mila gsin(q1) 一 


mg ,sin(q1) d- La sinCq, 十 qz)) 
根据 Pendubot 的 拉 格 朗 日 - 欧 拉 方 程式 (7. 320 ,可 求 得 其 中 的 各 项 表达 式 。 首 先 , 对 主动 
臂 , 可 得 


—m,làdi T milidi T mail, 十 ga ) -F 2m;li ls cos(q»)d, d 


m ;l,lacos(q5)d5 TI, 十 I;)di 十 I;4; 


re a m lad, c milidi T mis, 十 qz) 十 27zzlilecos(qds )qi — 2m;l, la sin(q;)q5qi + 


m;ljlcos(q,)q; — m;l,lasin(qs)q5d:; TG, IQ, Ba; 
— (mjl^, +m, (li +l 4-214105 cosq;) + CI, I2)gi + 
im; GL c lilacosq?) + I,)q; — 2m;l,l s sin(q;)dsq| — mil ,lasin(q?)q:d» 


JL 3K aP 
= — —— —0— mla gcoslg ) —m;gK(l,cos(q1) + La cosCq, + a32) 
9q, Iqı 99， 


=— Gnila- m;li)gcos(qi) —m;lagcos(q, 十 gz) 
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然后 ,对 于 和 欠 驱 动 臂 ,可 得 
IL š n š : 5 
—mila(qi -rq;) -- mili la cos(q;)qi. + I5q4 + Izq? 


dq: 


—[m;li --mililacos(q;) +I: ]qi + On; 1,)g; 


E - — [m;l5 4 millagcos(q;) + I5 ]qi - Gnsl3 4- I2)q5 — m;lilasin(Cq2)d;q| 
992 


aL oa . 
x — —m;lila4sin(q;)qiCq, +qs) — mila gcos(q, 4- q;) 
从 而 可 得 主动 臂 的 拉 格 朗 日 动力 学 方程 
d 3L JL 
EE Nc E 
" “di 9g1 dq, 
={m b +m, Qi +i, 4-20; 15cosq;? + XII, 4- 12)di + (m; G3 +lilacosg:) + I:}ġ: 一 


2m;l,lasin(q»)q5d; — m;l,lasin(qs)q5d; 十 


(mila 4 m;li?gcosq, 4 m;logcos(q, 十 gz) 


x malrlasin (qs ) d» dz 2c: — miljlasin (gz) geq 25 


其 中 ,一 2m,llssin Cga) qq; 
ms;ljlaosin(q;)q5(q4--q,). 
欠 驱 动 臂 的 拉 格 朗 日 动力 学 方程 为 
_d aL ta 2L 
dt 9q, 9q; 
— (m;ll, 4- mlilacos(q;) + I;24, + On;l5 + 129; + 


T2 


m;ljlsin(q;)qq; +#mMm:lagcoslqi +q2) 
最 终 ,可 得 到 整个 Pendubot 欠 驱 动 系统 的 动力 学 方程 ; 


Mud Oud TOO -r 
m, Em; GLO, 4-20, 0a cosq; ) 3- CI +I) 


(7. 36) 


qi 

其 中 ,rt 一 [rz tal 5750; -| IET =| 

l : j 4 qz " m; (3 -- lla cosq;) +I, 
ma (5, Hll acosg:)+ Iz ! —m;l,|lagsin(q;)qg; -—mjl,la4sin(q;)(q, +q) 

P „Cq q ) 一 à , 

mla tI: m ;»l,l;,;sin(q5)q, 0 
Gnj,la4--m;li)gcos(qi) d-m,l gcos(q,-q;) 
G(q0— e 
ml, gcos(q,-q;) 


7.5.3 Pendubot 动力 学 模型 


TU m 个 欠 驱 动 关节 的 关节 欠 驱 动机 械 系统 的 通用 Pendubot 动力 学 模型 如 下 ; 
M(q)9 十 C(q,q)g 十 GCC) =r (7. 37) 


Mi (q) Mi,(g) < Cias Co G, (q) 
其 中 ,MT( ) 一 $C * ) 一 GI ) 一 *, 
TU Mako diti ^ D ege 


A ^ 4—[4,.4,] € R^ 是 关节 变量 组 成 的 向 量 ,q, ER” 是 代表 主动 臂 的 向 量 ,qg。 是 代 


Ca (q T ) Clg T ) 


202 All 机 器 人 控制 系统 的 设计 与 MATLAB 仿 真 ， 基 本 设计 方法 (第 2 版 ) 


表 欠 驱动 臂 的 向 量 ,M(q) 是 nXn 对 称 正定 的 惯性 矩阵 ,C(q ,q )g 是 哥 氏 力 和 离心 力 的 向 
Htc, 是 控制 输入 力矩 的 向 量 。 

Pendubot 系统 包含 两 个 可 旋转 关节 ,电机 位 于 肩 关 节 , 肘 关节 没有 电机 了 驱动 ,如 
图 7-15 所 示 。 图 中 4 为 主动 臂 长 度 ,Lu 为 主动 臂 相 对 于 连接 点 到 质心 的 距离 ,!。 为 欠 驱 
动 辟 相对 于 连接 点 到 质心 的 距离 ,q, 为 主动 臂 相 对 于 水 平 坐标 轴 的 角度 ,gq, 为 从 驱动 臂 相 
对 于 主动 臂 的 角度 ,了 T 为 主动 臂 相对 于 质心 转动 惯量 ,1, 为 欠 驱 动 臂 相对 于 质心 转动 惯量 ， 
m, 为 主动 臂 质量 ,mm， 为 欠 驱 动 臂 质量 ,g 为 重力 加 速度 。 


Æ 7-15  Pendubot 系统 动力 学 模型 
二 关节 Pendubot 的 动力 学 模型 如 下 : 


E aa ec l= (7. 38) 
Ma Mz d; a Cn q? G: : 


其 中 ,Mi mimi T 215c0sg;) - CI; HI), Mi =Ma =m; (Q2 - 1,05 cosq;) + 


Ts Ma —m;li5-I1;,Cu = —millasin(q;2g5;»Cqy — —m;lilasin(q;2(q, +91) .C= 
mzlilizsin(gs) qis Cz — 0,G, — (milat mli)gcosgi t+ mlwgcos (gi gs),G, = 
mzlw Bcos(g Tq) 
通过 定义 参数 ,可 将 7 个 动力 学 参数 组 成 最 小 参数 集 的 5 个 新 参数 。 定 义 如 下 : 
0, =m, lh Hm li +I, 
m 


人 = 
0, =m;la H Mgt 
0, — mila 


pu 
Mi —0, +0: + 20,cosq; 
b. =M» =0, + 0,cosq; 
M; —0; 
则 动力 学 式 (7. 38) 变 为 
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0, 4-0, 4-20,cos(q,) 0,--0,cos(q,)] [d —q. 一 (q 十 di)| lg 
ý i E à | ME diis | Pl 


0, 4- 0,cos(q;) 0, d: di 0 d: 


pO i 的 


0.gcos(q, 十 qy) 


7.5.4 Pendubot 模型 的 分 析 


1. Pendubot 平衡 状态 的 分 析 

当 控 制 输入 力矩 c, 为 常数 时 ,Pendubot 的 平衡 状态 (ol ,qs,91,942) 二 (qi1,qs，,0,0), 此 
时 ,qi 一 0,dq* 一 0。 

则 由 式 (7. 39) 可 得 Pendubot 平衡 点 的 约束 方程 为 


Qigcos(qi)++Osgcos(qit qs) =T; 
(7. 40) 


0.gcos(q, 4- q;) —0 
平衡 状态 下 rı fJ i sh qi € (一 rr,qE( 一 rr, 则 可 得 到 平衡 点 处 
主动 臂 和 欠 驱 动 臂 的 角度 分 别 为 


qz: =n y 79 n 一 一 1,1,3 
(7. 41) 
" -arecos(77- 
X c, —0 时 ,Pendubot 处 于 平衡 状态 ,通过 上 式 可 以 得 到 Pendubot 在 4 种 平衡 点 处 的 
状态 分 别 为 
P, 二 一 元 250v0507 
P, —(qQ1593501:305) — C— x/2,1,0,0) 
P= tg dr —(x/2,0,0,0) 
P, = (qq) —6x/2.m,0,0) 
上 述 4 种 固有 平衡 状态 ,分 别 对 应 于 Pendubot 的 主动 臂 和 欠 驱 动 臂 同时 竖 直 向 下 、 主 
动 臂 竖 直 向 下 和 欠 驱 动 臂 竖 直 向 上 、 主 动 辟 和 欠 驱 动 臂 同时 竖 直 向 上 ,主动 臂 竖 直 向 上 和 欠 
驱动 辟 竖 直 向 下 四 种 状态 。 其 中 ,只 有 第 一 种 平衡 状态 是 稳定 的 ,其 余 三 种 都 是 不 稳定 的 。 
一 个 任意 小 的 扰动 都 会 导致 Pendubot 系统 远离 不 稳定 的 平衡 状态 ,特别 是 第 三 种 状态 
P:=(r/2,0,0,0) 是 最 难以 保持 的 。 
因此 ,需要 设计 既 能 抗 干 扰 而 且 重 棒 性 又 强 的 控制 律 ,使 得 Pendubot 系统 能 在 其 不 稳 
定 平衡 点 处 保持 稳定 。 
2. Pendubot 非 完整 约束 性 
根据 非 完 整 约束 阶 数 的 不 同 , 欠 驱 动机 械 手 可 分 为 一 阶 和 二 阶 非 完 整 系统 ,其 中 ,前 者 
具有 速度 约束 不 可 积 , 后 者 具有 加 速度 约束 不 可 积 。Pendubot 系统 被 看 成 是 一 类 二 阶 的 非 
完整 约束 系统 ,具有 加 速度 约束 不 可 积 的 特性 。 欠 驱动 机 械 系 统 的 可 控 性 和 稳定 性 与 可 积 
性 密切 相关 。 对 于 非 完整 约束 系统 ,控制 难点 在 于 无 法 用 光滑 反馈 使 系统 在 平衡 点 附近 局 
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部 渐进 稳定 。 针 对 不 同 的 控制 目标 ,设计 非 光 滑 反 馈 稳定 的 控制 策略 。 针 对 Pendubot 系统 
的 控制 一 般 分 为 起 摆 和 平衡 两 个 阶段 ,通过 切换 控制 达到 对 该 非 完 整 约束 系统 的 有 效 控制 。 

通过 式 (7. 39) 的 第 二 行 , 可 得 到 在 Pendubot 系统 中 存在 的 二 阶 约束 方程 如 下 : 
(0, 4- 0,cosq;)q, 十 020 4- 0Ó,sin(q,) qi 4- 06,gcos(q, 4- q;) =0 (7.42) 


7.5.5 滑 模 控制 律 的 设计 
以 最 难以 保持 的 不 稳定 倒立 平衡 点 P, 为 例 , 设 计 滑 模 控制 律 ,使 其 在 有 限时 间 内 到 达 
平衡 点 ,并 且 保持 稳定 。 由 于 P; 一 (x/2,0,0,0), 则 各 个 状态 的 跟踪 误差 为 


el =q — 7/2, el 一 qi， E —QqQ2* és =q: 


由 式 (7. 39) 可 得 


qi aF 
P -ma Ce- cpi - e] (7.43) 
q2 
Xd. Mays 0,--0,43-20,cos(q;) 0,--0,cos(q;) onm 
: 3 Bat 0,cos(q;) 0, ” dd VS que 
HF 
á M» —M 
M(q)' ET "UD IRR " a 
|M | Ma Mz 一 MoMa — M4 My 


R =q: — (di 十 qi) H Wr +0; g cos(q, bol 
=; singz „(F 
di 0 d; 0.gcos(q, 十 gz) 


. — 24:d| —di 0,gcosq, - 0,gcos(q, q2) 
— Ü,sinq; + 
2 0.gcos(q, qs) 


qi 
4 H(q.q)—[h, h] W 
hı — — 20,sin(q;)q;q; — Ü,sin(q;)q; 4- 0,g cos(qi) 4- 0,g cos(q, 4- q;) 
h: —0,sin(q;)qiq; 4-0; g cos(q, 十 02) 
从 而 
- ET 1 M» 一 Ma| [tu — h, 
M(q) [e —€(a 40d — GGD]— r7 MM, bra M, | ie | 
1 Mati 一 MA 十 Mi 
MM» 一 MbM2 desit 十 Mh d 


1 ( — Mh, - My; "n | 22 | ) 
= j5 
Mj M5 一 MoM Mah, —Myh; — M4 i 


di — fi Tb td, 

qz: =f: bti +d: (7. 44) 
—MahstMyuh. , - M» y, m Mota Mabi 二 去 
MaMa MoMa ^ MMe MeMa ^^ MiMe MpMa ~” 


则 式 (7. 43) 可 写 为 


其 中 , fi = 
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一 Ma 
Ma M;,; — Mi,M, 


控制 的 目标 为 qix/2.q,—0.q0,—0,q,—0, XE X. IE BS PROCU 


,di d; 是 加 在 控制 输入 端的 干扰 。 


s =a, 十 Mel Haze: 十 zey 王 aiel Haz: Fs, (7.45) 
其 中 ,s, 二 Ajel 十 XAzes sal AÀA a TIL A; 分别 为 滑 模 面 系数 。 
于 是 
3 三 alel Hae: 十 ,一 aidl Haga HS: 
=f; F bitr td) taz fa tbati tada) d$, 
= laby aber Ferfi terdir Farf starda FS 
设计 滑 模 控制 律 为 
npe 十 5: 十 ysgn(s) 十 ks) (7. 46) 
其 中 ,w=|aildi 二 lasld;,1di|<di,1d;|<d;,,y>0,k8>0。 
于 是 


S= aë, Faze: F5, = al0 Hag 45, 
= a, Cf, bc, +d1) +a: (fs boc, +d) ts 
= (a;b; 4J- a5b,)r, Haifi d- a5 f o 十 5 Fardi d asd; 
— — (gsgn(s) -- ks) c aud, ta:d: 


inis l4 
设计 Lyapunov PR JJ Veces , 则 
V=sf =—7 | s |— ks’ +slaid, +azd2) K— ks? — —2kV 


采用 引 理 6. 1 ,针对 不 等 式 方程 Vs Vf a— sf 70 OS 


k 
=7 (to? 


VG)xe UN Cu) 

可 见 ,V(Gz) 指 数 收敛 至 零 ,收敛 速度 取决 于 &。 指 数 项 一 As 能 保证 当 s 较 大 时 , 滑 模 函 
数 能 以 较 大 的 速度 趋 近 于 滑动 模 态 。 因 此 ,指数 趋 近 律 尤 其 适合 解决 具有 大 阶 路 的 响应 控 
制 问 题 。 

注 : 由 函数 的 表达 式 可 知 , 上 述 分 析 中 , 滑 模 控制 律 只 能 实现 对 一 0 的 鲁 棒 控制 ,但 
不 能 实现 qi 一 r/2,q1 一 0,q >0,q: 0. 

为 了 实现 qi 一 r/2,q1 一 0,qs 一 0,q; 一 0, 需 要 进行 闭环 系统 分 析 , 从 而 通过 设计 滑 模 函 
数 中 的 系数 ,实现 控制 的 目标 。 

由 于 基于 Hurwitz 稳定 性 判 据 的 控制 器 设计 方法 不 具有 和 鲁 棒 性 ,因而 在 下 面 的 闭环 系 
统 分 析 中 , 取 di 王 0,d :一 0。 


7.5.6 闭环 稳定 性 分 析 


通过 上 面 分 析 可 知 ,ss x0 成 立 , 则 存在 tts sim. = s=0 时 ,有 Y 一 alei 十 Aiei 十 
aze ;十 A2es 二 0, 由 于 el 一 qi 一 r/2,e 一 qilyez 一 q? TIT 
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Qa191 十 A1(gi 一 x/2) 十 azqs 十 Mg 一 0 (7.47) 
BT 
0, 0, + 0.cosq. aĝ; — a; (0, + 0,cosq;) 
KUR dg Eg en 0,0, — 0; cos* q io. 0,0, — 0:cos'q; Hu 0,0, 一 03cos as 


MaM» — MM = (0, 十 9; + 20scosqs)0; -—1 (8, 十 0;cosg;s "a —0,0, — f: cos'q; 


M s—0 时 ,控制 律 为 T= — Ca, f, as f ats) M 


1 
abı Fab: 
b: 
aibi 十 azp02 
_ flab +azb:)—b: Ca fı b as f o H5) _ fzaib! — bza fı — b5, 
, wib 3- azb; E abı Farb: 


qz = fibt td: =f2— (a, f, Faz f; 2r 


由 于 

M5 (Mah, — Muhs) Mat Mab, Mah) 
'(MuMs —MuMa)! " (MaMy MaMa)? 
My (Mh, — Muhs) Ma (一 MD 十 Ms) 


f;ajb, —b;a, fi SQ 


M (MaMa 一 MD MD 
| Ma CC Mihi) + Ma lh) 
-i (Mi M» — MM; 
— (M» Mu — Mo M1)h, hs 
MM MMa) MM — MM 
aibi 十 asbs —a, Mz +a: Z Ma = Mew Ma 
Mu Mz — Mi Mo, Mu Mz —M;Man MM» — MM 
则 
hs 一 AM， 
,fiab bmfi—bii, "MiMs —MaMn MM -MoMa ` 
a,b, d- ab; a, M5 —a;M4 a,M; —a;Ma 
M Mz —M,iM; M Mz — Mi My, 
—a,h; Ms, 


» «iMi — a; Mg a, M, —a;M; 
将 Ah JJM2a a, M; 一 azM2 —a,0, —a;(0,; -F0.cosq; ) 代 人 可 得 


—a;(0,sin(q;)qi1qi 4- 0,gcos(q, + q;)) (0, 4- 0,cosq;)s, 
SE 210, — a, (0, + 0,cosq;) ai F =a O 4- 0,cosq;) 
由 以 上 分 析 可 得 
» Ài E «X . Oz. ELT 
qu -— 2 n =) a a a (7.48) 
M — a, (0,sinCq;)qiq; 4-0, g cos(q, +q:)) 0, 4- 0,cosq; Qd. Ad 
WT a,0, — a, (0, + 0,cosq;) w= a F acosa) ML ade 


(7. 49) 
4 Vidz Y2 =q 2Y =q 一 x/2, 则 可 得 如 下 降 阶 系统 
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Yi ys 
Ai 5 À ? 
—S* sino E 2 n y.) +0sgeos(y1 +5 +y) | 
"T Ql a, Qi 2 4 
zu aiz — a5 (0, + 0,cosy,) 
0. -F 0.cosy, À1À2 À iao À1À1 
ai: — a: (0, + 0,cosy,) ai » t. al )>: ai - 
À» À 
ya 一 一 一 yi b i — ya CT. 50) 
QI Qi Ql 


下 面 考虑 Pendubot 系统 其 不 稳定 倒立 上 平衡 点 了 :一 (ai ?Gd2 siis Q5) — (/2,0,0,00 k 
近 的 控制 问题 ,首先 针对 式 (7. 50) 进 行 线性 化 ,由 于 y 10 y270. y 770. DUI 


T a 
0. g cos(y, Te +ys)=— 6: gsinCy, Ty) 2 —0,gyi — 058I: 


| ei(,gy, c Osgys) 0; +0; _ A412， +(a ur Ai | 
T ars —as(05 + 0,4) ails —:a5(0; 3-04) Ql 74 * Ql Ts Ql Js 
则 式 (7. 50) 可 近似 为 系统 y= 二 Ay ,其 中 
0 l 0 
An An As 
A= Gt 81 
Àe Ca EET 
Ql Ql Ql (y 70. y, 70. y, 70) 
AIA， "EDT AT 
a,6,g — (0+8) (~ ) (0,--6»(4,—À 5) abg — (067 
Ql Ql a, 
其 中 ,A, = ,Az = 


a,0, —a; (0, F0;) aj mE eT eR C 

当 状 态 矩 阵 4 满足 Hurwitz 稳定 性 判 据 时 ,对 于 正定 矩阵 Q ,一 定 存在 正定 矩阵 已, 使 

fd A'P--PA — —Q, ^ Lyapunov 函数 为 wW 王 并 Py M 
V, — y! Py 4- y! Py — CAy)! Py + y! PCAy) 
—y'(A'P--PA»y ——y!Qy «- ly'Qy ll; SÀ mQ) lly ll? <0 

其 中 ,4,,(Q) 是 正定 矩阵 Q 最 小 特征 值 。 

根据 LaSalle 不 变性 原理 , 当 V, —0 时 ,y 三 0, 则 + 一 2 时 ,y 习 0, 系 统 的 收敛 速度 取决 
TO. 

根据 y, =q: y2: 74:374: 一 x/2, 则 toll ,q,—0,0,—70,q,7n/2. 考虑 到 e = 
全 一 fi 三 diver=daes=ga BT t>o, s =at 十 Njes Farés T A;6; 0, M 


q1770. 
7.5.7 基于 Hurwitz 的 参数 设计 


根据 A 满足 Hurwitz 稳定 性 判 据 ,求解 满足 滑 模 面 参数 w Ai 、a, 和 4,。 根 据 式 (7. 47)， 

当 系统 的 运动 轨迹 到 达 滑 模 面 时 ,四 个 参数 中 只 有 三 个 是 独立 变量 , 另 一 个 是 非 独立 变量 ， 
不 妨 取 a 二 1, 则 有 

0sg — Àd: C0; +03) . (0, 7-0,)(4; = Ara) 


m Bs |m aal +0) ” Ag = 0, —a5€0; 1-04) 


fg — A10, 9-04) 
LI A; 二 一 一 一 一 一 一 一 一 
0; — a; (0, Tt 0s) 
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且 
0 1 0 
A—|An A» An 65.521 
一 1 —&à; 一 
则 
À 一 1 0 
| MT 一 4 | 王 | 一 Ai 0 和 一 Az As 
^s a» à +à: 


—A( — An) A HA) HA An HÀa: An — (A +A An 
=)" 4-0, — A424! — AnA HAA HÀazAn — (A +à An 
—A5 + A, — An)? C^ And Ha:An —An)A d C— A1A HAA) (7.53) 
A 的 特征 方程 为 
13 十 (一 Ap) :十 (一 AAA 十 asAs 一 An) 十 (一 1 人 A HAA) —0 
三 阶 系统 特征 方程 一 般 形式 为 


ao Haii? 十 as 十 ws 一 0 (7. 54) 
从 而 得 到 
ag =l 
äi =À m~ Aa 
a; ==Å n Für An ~= Áa 
ai ——A4AÀ4 T AsÁg (7.55) 
REX 6-0, 0,06 N JI An m Et A La at gE: 
TE 
0(,—2a,4,) (0 一 cg) 一 0 一 Xiaa) Ar — A40 
i 
0(1, —24,2a;) 0g —A10  O,g —À,À,0 
qe Ue iN wer 
(0—0A13$ --0Aia; d-0,ga; — Ài 0a, —0,g 十 A1A20 
N 
_ 0,ga; —0,g  0,g(a; —D 
N N 
a =mi, SETAA, m 28. 
一 (bg 一 Mg) 十 is(bg — AO Og (4, —ÀD 
N N 
dia HEUS appa, 一, 即 as0s 十 0sg mes Osa N 
"E xal (7.56) 


zr 0.g 十 as0 
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Aj;0,—440 —A,8,—25,(8,-F04,) — (Ai1—4530,—2A50& ， a, N 34,0, 
a Ns = 7, , 由 
0sg(As —À1)..., asN a, N -A,0, a4 N a, NO, 
i 2 一 1 一 . Ji e MIA B e a Nm 
a; N 可 得 4 一 4， e 则 à; 0. 即 A30, ^» a, N 
(a40,g 3-a40,) N 
"c eti 
58 
(a,0,g +a;0)N 
À: = ne (7.57) 
QN 
M ei (7.58) 
0.g 
为 了 抑制 扰动 ,实现 y-—AQC—AO ,需要 将 4 在 左 半 面 的 极点 设计 得 大 些 。 
7.5.8 仿真 实例 


被 控 对 象 取 式 (7. 44) ,根据 机 械 手 的 物理 参数 , 取 0 三 0. 0104, 0, = 0. 0052, 
8, —0. 0047,0, 一 0.0805,0; 一 0.0344, 取 d, =0,d;=0. 

ika +3) =0 设计 4 的 极点 ,此 时 3 十 9%M2 十 27A 十 27 王 0, 可 得 a, =9,a, — 27, 
as 一 27。 根 据 式 (7.56) 一 式 (7.58) 设 计 az Az 和 41。 控 制 律 式 (7. 46) H, He — 10, fX 
7 一 xldi 十 asd 十 p 设计 7。 针 对 不 稳定 倒立 平衡 点 P;, 使 初始 位 置 为 (gq gq) = 
(x/2 一 0.01,0,0,0) ,为 减 小 控制 输入 拌 振 , 仿 真 中 使 用 边界 层 厚度 0. 05 的 饱和 函数 近似 代 
蔡 符号 函数 ,仿真 结果 如 图 7-16 一 图 7-18 所 示 。 


angle of actuated arm,q1 
i = 
D e 


time(s) 


angle speed of actuated arm,dq1 
b 
3 


5 10  ， 15 20 25 30 
time(s) 


7-16 ”主动 臂 角 位 置 和 角速度 输出 
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angle speed of underactuated arm,dq2 angle of underactuated arm.q2 


0 5 10 15 20 25 30 
time(s) 
0.3 
0.2 
ol 
0 
-0.1 
0 5 10 15 20 25 30 
time(s) 
图 7-17 欠 驱 动 臂 角度 和 角速度 输出 
0.05 
0.04 
= 003 
E 
3 | 
a Il 
Z 002 
E 
g 
8 
0.01 
0 
Dola 5 10 15 20 25 30 
time(s) 
图 7-18 控制 输入 信号 
仿真 程序 如 下 : 


(1) Simulink 主 程序 : chap7_5sim. mdl。 


control input2 
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(2) 控制 器 子 程序 : chap7_5ctrl. m, 


function [sys,x0, str,ts] = spacemodel(t,x, u, flag) 
switch flag, 
case 0, 
[sys, x0, str, ts] = ndlInitializeSizes; 
case 3, 
sys = ndlOutputs(t, x,u); 
case {2,4,9} 
sys-[]; 
otherwise 
error(['Unhandled flag = ',nunm2str(flag)]); 
end 
function [sys, x0, str,ts] » mdlInitializeSizes 


sizes = simsizes; 


LU 
ÉÓ! mi eoo 
ho 


sizes.NumContStates 


sizes.NumDiscStates 
sizes.NumOutputs 


sizes.NumInputs 
sizes.DirFeedthrough 
sizes.NumSampleTimes 


sys = simsizes(sizes); 


x0 = []; 
str = []; 
ts = [00]; 


function sys » mdlOutputs(t, x, u) 
ql = u(1);dq1 = u(2); 
q2 = u(3);dq2 = u(4); 


thetal = 0. 0104; theta2 = 0. 0052; theta3 = 0. 0047; theta4 = 0.0805; theta5 = 0.0344; 
g=9.8; 


m11 = thetal + theta2 + 2 * theta3 * cos(q2); 
m12 = theta2 + theta3 * cos(q2); 

m21 = m12; 

m22 = theta2; 


hl = -2* theta3 * sin(q2) * dq2 * dql - theta3 * sin(q2) * dq2^2 + theta4 * g * cos(q1) + theta5 * 
gx cos(ql + q2); 

h2 = theta3 * sin(q2) * dql^2 + theta5 * g * cos(q1 + q2); 

mO = m11 * m22 - m21 * m12; 


f1 = ( — m22 * hl + m12 * h2)/m0; 


bl = m22/m0; 
f2 = (m21 * hl — m11 * h2)/m0; 
b2 = - m21/m0; 


al = 9;a2 = 27;a3 = 27; 

theta = theta2 + theta3; 

alfal = 1.0; 

alfa2 = (a2 * theta2 + theta5 * g)/(theta5 * g + a2 * theta); 
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N = theta2 - alfa2 * theta; 
lambda2 = - (al * theta5 * g + a3 * theta2) * N/(theta3 * theta5 * g); 
lambdal = lambda2 - a3 * N/(theta5 * g); 


el = ql— pi/2;del = dql; 
e2 = q2;de2 = dq2; 


s = alfal * del + lambdal * el + alfa2 * de2 + lambda2 * e2; 


dl max- 0. 0;d2_max = 0. 0; 
xite = alfal * dl max + alfa2 * d2_max + 0.10; 
k= 10; 


% Saturated function 
delta = 0. 05; 
kk = 1/delta; 
if abs(s)» delta 
sats = sign(s); 
else 
sats = kk * s; 
end 
sr = lambdal * el + lambda2 * e2; 
dsr = lambda1 * del + lambda2 * de2; 
ut = - l/(alfal* bl + alfa2 * b2) * (alfal * f1 + alfa2 * f2 + dsr + xite* sats +k * s); 
%ut= — 1/(alfal * bl + alfa2 * b2) * (alfal * f1 + alfa2 * f2 + dsr + xite * sign(s) * k* s); 


sys(1) = ut; 
(3) 被 控 对 象 程序 : chap? 5plant. m, 


function [sys,x0, str,ts] = spacemodel(t,x,u,flag) * From chapl3 2plant.m in PID Book 
switch flag, 
case 0, 
[sys, x0, str, ts] = mdlInitializeSizes; 
case 1, 
sys = ndlDerivatives(t,x,u); 
case 3, 
sys = ndlOutputs(t, x, u); 
case (2,4,9) 
sys- []; 
otherwise 
error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0, str, ts] = mdlInitializeSizes 


Sizes - simsizes; 


sizes. NumContStates 
sizes.NumDiscStates  - 


4 
0 
sizes.NumOutputs = 4; 
sizes. NumInputs =. 
0 


sizes.DirFeedthrough = 
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sizes.NumSampleTimes = 1; 
SyS 7 simsizes(sizes); 

x0 = [pi/2-0.01,0,0,0]; 
str = []; 

ts = [00]; 


function sys = mdlDerivatives(t, x, u) 
thetal = 0.0104; 

theta2 - 0.0052; 

theta3 = 0.0047; 

theta4 - 0.0805; 

theta5 - 0.0344; 

g=9.8; 


ql = x(1);dq1 = x(2);q2 = x(3);dq2 = x(4); 


m11 = thetal + theta2 + 2 * theta3 * cos(q2); 
m12 = theta2 + theta3 * cos(q2); 

m21 = m12; 

m22 = theta2; 


hl = -2* theta3 * sin(q2) * dq2 * dq1 - theta3 * sin(q2) * dq2^2 + theta4 * gx cos(q1) + theta5 * 
g * cos(ql + q2); 
h2 = theta3 * sin(q2) * dq1^2 + theta5 * g * cos(q1 + q2); 


mO = m11 * m22 — m21 * m12; 


f1 = ( — m22 * hl + m12 * h2)/m0; 
bl = m22/m0; 

f2 = (m21 * hl - m11 * h2)/m0; 
b2 = - m21/m0; 


tol=u(1); 

sys(1) = x(2); 

sys(2) = f1* b1* tol + dl; % ddql 
sys(3) = x(4); 

sys(4) = f2 + b2 * tol + d2; % ddq2 
function sys = mdlOutputs(t, x, u) 

sys(1) = x(1); 

sys(2) = x(2); 

sys(3) = x(3); 

sys(4) = x(4); 


(4) 作 图 程序 : chap? 5plot. m, 


close all; 


figure(1); 
subplot(211); 
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plot(t,x(:,1), 'k', 'linewidth',2); 
xlabel('time(s)');ylabel('angle of actuated arm,q1'); 
subplot(212); 

plot(t,x(:,2), 'k', 'linewidth',2); 
xlabel('time(s)');ylabel('angle speed of actuáted arm, dqi'); 
figure(2); 

subplot(211); 

plot(t,x(:,3), 'k', 'linewidth',2); 
xlabel('time(s)');ylabel('angle of underactuated arm, q2'); 
subplot(212); 

plot(t,x(:,4), 'k', 'linewidth',2); 


xlabel('time(s)'); ylabel('angle speed of underactuated arm, dq2'); 


figure(3); 
plot(t,tol(:,1), 'k','linewidth',2); 
xlabel('time(s)');ylabel('control input, toll'); 


附录 


1. 机 器 人 动力 学 表达 式 (7. 13) 的 推导 
根据 牛顿 力学 原理 ,机 械 臂 动力 学 方程 形式 为 


i 十 2ecos(qs) 十 2ysin(qs) f --ecos(q;) + ysin(q;) 


B +ecoslq:) + gsin(q;) B 
€Y, 4 7Y: + (a — B o eie;cos(qi) H 
cY, 十 7Y， — in 


其 中 ， 


Y 
Y. 
Y, =sin(q:)qi 十 excos(dq +q) 
Y, — —cos(q;)di 十 ezsin(q +qz) 
el mil,l,—I,—mili 


es 二 g/li， 8 为 重力 加 速度 


由 Y; ;Y, 3X 3Y4 的 定义 ,可 知 


则 


€Y, T 3Y; 4- Ca — B - eiDescos(qi) 


essin(q, +q2)) + Ca — B o- eiJescosCq,?) 


gyessin(q, 十 gz) 十 (aa 一 8 十 el)ezcos(91) 


Jkl 


(1) 


; — —2sin(q;)qi1d; — sin(q;)q$ 4- e;cos(q, +q) 
; —2cos(q5)41qQ; 十 cos(qy)di -- e;sin(q, 4- q;) 


=e (— 2sin(q;) d.d; — sin(q;24$ -F escos(q, +q2)) d- 3 (2cos(q;) qid; d- cos(q,)q; + 


— (— 2esin(q;) + 29cos(q,)) sd, + C— esin(q;) 十 7cos(qs))9: -- ee; cos(q, 十 gs) 十 


eY, 4- 3Y, —e(sin(q;)di -- e;cos(qi d- q;)) d- qC— cos(q;)41 4- e;sin(q; +q2)) 


= (esin(q;) — gcos(q,)) di -- ee; cos(q, +q) + ge;sin(q, +q2) 
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eYi 十 ynY; 十 (a 一 BB 十 ei1)escos(g1) 
| eYs+ 7Y | 
[- 2esin(q;) 4-29cos(q;))d,;  (—esin(q;) 十 didis H 4 
(esin(q;) — gcos(q;))q, 0 d: 
ee; cos(q, q2) + ge;sin(q, d- q5) + Ca — B -- eiDescos(qi) 
| ee;cos(q, 十 gz) 十 Tezsin(qi 十 gz) | 
于 是 式 (1) 可 写 为 
[la 二 2ecos(q;) 十 2ysin(qs)  B-d-ecos(q;) + TN M " 
| B -- ecos(q;) + ysin(Cq;) B d: 


[一 2esin(q;) 十 27cos(q:))g。， (—esin(q:) 十 Tcos(qz))9， H 4 
(esin(q;) — ycos(q;))q, 0 


[tes cos(Cq, +q2) + ye;sin(Kq; 十 qz) 十 (ac 一 8 十 en | 网 
| ee;cos(q, 十 gz) 十 Tezsin(ql +q2) 


T2? 


> 


+ 2ecos(q:) + 2ysin(q:) B+ecoslq:) + di 


a 
AM (dg ) 一 
Biecos(gs) gsin(q;) B 


. - 2esin(q;) --29cos(q;))q,; (—esin(q:) 十 icis 
C(q.q) — 
(esin(q;) — ycos(q,))q, 0 
ee; cos(q, 十 qz) 十 7ezsin(ql 十 qz) 十 (一 8 十 el)eszcos(q1) 
Supr | eesCOS(q1 十 qs) 十 yezsin(qi 十 qs) | 
则 式 (1) 可 写 为 标准 的 机 械 手 动力 学 方程 
M(gq)q+i+C(qg,g)qg + G(q) ^c (2) 
其 中 ,qg 一 [gl g] 一 [ri rz] 。 
2. 机 器 人 动力 学 方程 线性 化 的 推导 
A) T 4-46 BE 65 45 3E 4E EST 28 


^ 


x â + 2€cos(q;) + 29sin(q;) B + £cos(q;) + jsin(q;) 
B -- €cos(q;) + jsin(q;) B 
F p 2ssin(q，) + 29cos(q;))d; ipsi dudas 


(£sin(q;) — jcos(q;))d, 0 


A pm 十 qs) 十 ge;sin(q, 十 gz) 十 | 
€e;cos(q, 4- q;) 十 nessin(gi Faa) 
则 相应 的 估计 误差 为 
"E a 4- 2€ cos(q5) 4- 29 sin(q;) T- € cos(q;) 4- 7 sin(q;) 
M-M-—M-— a Siq» 7 Sinig: B q2 7 qz 
B +Ecoslq:) 4- 3 sinCq;) B 
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则 


Mq, + C4, +G 


(B 4- €cos(q;) +7 sin(q;))qu 十 ou 


ja + 2€ cos(q;) - 27 sin(q;)) qu + (B +E coslq:) +7 sin(q;)) qus 


[ — 2€ sin(q5) +27 cos(q5)) dd 二 C— € sin(g5) + 5 cos(q;)) dou 


KG'sin(q?) — 7 cos(q5)) dida 


T E (— 2€ sin(q;) + 25 cos(q,))4; 
(€ sin(q;) — $cos(q,))q, 


[(qu d- ezcos(q;)) à 十 (qu — escos(q 0) B 


(— €sin(q;) + $ cos(q;) * 
0 


B di us €escos(q, 十 gz) 十 Tezsin(dqi 十 gz) 十 (& — BDe;cos(qi) 
€escos(q, Hq) + Jezsinlqı 十 gs?) 


E 
EE 


+ G2cos(q;)qq 十 cos(dqy)du — 2sin(q;) qsquy — sin(qo2doqu; + escos(q, +q:))E 


= 十 (2sin(gs) ga 二 sin(gs) qu + 2cos(q5)d5 qu + cos(q;) do du; 十 essin(q) 十 qz))7 


0. -- (a Hga) + (cos(gs) gu 十 sin(qs)didu + escos(qi +q:))E 


十 (sin(qy)gu 一 cos(qy)gidu d- ezsin(q, +q:)) 7 


qa 十 excos(qgi) qa — escos(qi) 


0 
B 
E 
7 
上 式 可 写 为 
其 中 ， 
Yq qasqa) 


qa + escos(qi) 


da Ha 


Mà, + Cda +G -Y(q.d qo d)p 


qa Hae 


qa — escos(qi) 


2cos(q;) qq + cos(q;)q 

im 2sin(q; )dsqa = sin(q; ) qs qa 二 
e;cos(q, 十 g2) 

cos(dqy )gu + sinfga dida 十 
escos(q, 十 gz) 


2cos(q;)qa + cosg )9 

— 2sin(q;) ds qq — Sin(q2 )qsqu 十 
e;cos(q, 十 gs) 

cos(gy)gu + sin(gs) dida 十 
escos(q, 十 qz) 


2sin(q;) qa + sin(qs )gqu 十 


2cos(qs)qsqu 十 cos(qz )gydu + 


e;sin(q, 十 dz) 


sin(q; dqa — cos(q; )djida T 
e;sin(q, d- q5) 


2sin(q, )gu + sin(q;2qu 十 


2cos(g; )d:da + cos(q; )dsde T 


e; sin(q, 十 gz) 


sin(q; )da — cos(q; )jida T 
e; sin(q, 十 qz) 


(3) 


(4) 
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cum 


上 式 中 采用 q,..4, X3 q.d, TAY d ,91,4;) 的 表达 式 。 


Y(q.q.q,.q.) 


2cos(q5)q, 十 cos(qy)gu 2sin(q;)q,, 十 sin(q2)q, 十 


qa t es;cos(qi) ga 一 ezcos(qi) — 2sin(q;)d;d, — sin(gs)GsG2 +  2cos(q;)ds d, -- cos(q) ds d 十 


[1] 
[2] 


C9] 


[10] 
[11] 


escos(q, +q) e;sin(q, 十 gz) 
"EA cos(q,)q, + sin(q;)qdiq4 + sin(q;)q, — cos(q;)d.q4 十 
0 Qu 十 ge : 
e;cos(q, 十 q2) essin(q, 十 qz) 


(5) 
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CHAPTER 8 


8.1 单 力 辟 机械 系 统 的 鲁 棒 自 适 应 控制 


通过 引入 直接 自 适应 控制 的 思想 ,采用 基于 Lyapunov 直接 法 的 鲁 棒 模型 参考 自 适 
应 控制 方法 ,可 以 在 具有 参数 不 确定 性 和 未 知 非 线性 摩擦 特性 的 情况 下 ,使 跟踪 误差 
趋 于 零 , 其 优点 在 于 不 需要 建立 摩擦 模型 ,不 需要 精确 的 摩擦 参数 ,而 只 需 摩 擦 的 上 
a”, 


8.1.1 问题 描述 
不 确定 单机 械 臂 为 
I0 -- (d 4- 0,0 +8,0 4- mglcos 0 =u — f (0) (8.D 


其 中 ,9 为 系统 输出 转角 ,1 一 他 ml ,1 为 转动 惯量 ,mg 为 重力 ,u 为 控制 输入 ,了 (6 ,x ) 为 未 


知 的 非 线 性 摩擦 ,7 为 质心 距 连 杆 的 转动 中 心 ,qd 为 连 杆 运动 的 理性 摩擦 系数 ,6, Du S TERRE 
擦 系数 的 不 确定 值 ,6, 为 弹性 摩擦 系数 。 

如 果 机 械 臂 的 运动 平面 与 水 平面 平行 , 则 机 器 人 运动 方程 中 的 重力 项 可 以 忽略 。 则 
式 (8. 1) 变 为 


j4 27g 00g - — £O» 
即 
0 +a --a,0 — BCu — f CÓ) (8. 2) 
h a = EČ aÈ g a, van 为 非 负 的 有 界 实数 ,8 为 正 实数 。 


假设 | f(0)| 二 F,, ,对 式 (8.2) ,引入 参考 模型 如 下 : 


Ön Hain Haon —br (8.3) 
其 中 ,0。 为 模型 输出 ,x 为 指令 输入 ,al av、 为 正 实数 。 
定义 误差 信号 为 
e 一 0 一 0 (8.4) 
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控制 目标 为 当 a, 、a。 和 BB 为 未 知 时 ,通过 设计 控制 律 ,使 得 对 于 任意 初 态 ,! 一 c 时 ， 


e()-0.é (t)>0., 


8.1.2 鲁 棒 模 型 参考 自 适 应 控制 
由 式 (8. 2) 和 式 (8.3) ,可 得 
EN nd, = E TY, 


即 
€ +a Haie —br — Bu -- Bf CÓ) + (a, — a,)0 + (a, 一 ao)0 (8. 5) 
定义 向 量 x — [ee] ,得 到 上 式 的 状态 空间 表达 形式 为 
beai- Phet iis 8.0 
' : 1 
其 中 ,A==br 十 Bf ()4-(,—a,)0 --(2,—a,)0,A— | | ,Z 一 一 E H : 
ao Tai 
由 于 矩阵 4 的 特征 值 具 有 负 实 部 , 则 存在 正定 矩阵 已 和 @ ,使 得 下 式 成 立 : 
A'P --PA ——9 (8. 7) 
定义 辅助 信号 6 为 
é-[0 1]Px=[0 1] M ai d — pe + pé (8. 8) 
2 3 e 
Rup.p— b " . 
2 E 
取 控 制 律 为 
u 一 Ar 十 RiO 十 As0 十 也 (8. 9) 


其 中 ,k,。、k， Rh. 为 待 调节 的 增益 系数 ,v 为 鲁 棒 项 。 

定理 8.1"” 针对 系统 式 (8. 6) ,采用 控制 律 式 (8.9), 若 增益 系数 自 适应 律 和 和 鲁 棒 补 偿 
项 设计 为 

ko =A êr, RQ—A,60, R,—2A,€0. v—F,sgné (8. 10) 

其 中 »sgnC*) Jg fF PRÉC. A Ai v" 为 正 实数 。 则 对 于 任意 的 Qoa „f ) 及 任意 的 初始 条 
Fe A e (zt) 是 有 界 的 且 渐 进 收 敛 于 0。 

参考 文献 [3] 的 分 析 过 程 ,对 定理 8. 1 分 析 如 下 : 

定义 Lyapunov 函数 为 


l T 1 2 1 2 
ee — Bk.) T P e 2 
VG) 2* POLES Bk ta (a a Bk1) 十 


1 2 
WE — ai — BR 
由 于 


l r r lar Ly T l r 
(sx Px) —MEPx Lx Pi— (Ax +Z)" Px HaT PEZ) 
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- GA 十 Z')Px + Fx PAx +Z) 


= Qr P 十 PA)x 十 QU Px 十 xIPZ) 


= P Qox--x'PZ 


aa Pl. PaT aoo 


xTPZ = e e Sh la- — Bu) — &(A — Bu) 


— éCr HBf 4- Xa, — a4,)0 + las — ao) — BO, r 4- &,0 + k,Ó + v) 
—ér(b — kj) +ê0las — as — Bk) +é (a —a, — Bk) --eBCf — v) 
则 


六 一 一 了 xzTOx Hé r(b — Bk.) 十 20(a — a, — Bk.) Hea — a, — Bh.) -HBCf —v) — 
Èa È, È 
q TBko j= ja — Bk) 一 iG. — a1 = RO 


= x"Qx + (êr i -pko + (0 — B) e as poH 
(4 E) a -a -po Ha 一 
将 式 (8. 10) 代 入 ,得 
=— Fx"'Qx +é8(f (0) — v) 
由 于 
eBCf — v) — eBCf — Fsgne) —Bf — BF, |e |< O 
DU 


À max ( ) z 
50^ lelto 


其 中 ,wx(2) 分 别 为 矩阵 Q 的 最 大 特征 值 , | e | 为 2 范 数 。 


由 于 当 V 二 0 时 ,x 三 0。 根 据 LaSalle 不 变性 原理 ,闭环 系统 为 渐进 稳定 , 即 当 上 一 co 
时 ,zx 一 0, 即 e(z) 和 6 GO EE ABS Bir SUCT 0。 


由 于 V 宇 0,V 志 0, 则 当 t1 一 oo 时 ,V AR, AE koki Ak AR 


; 1 
Vs- zx Qx < 


8.1.3 仿真 实例 
被 控 对 象 的 动态 方程 为 


0 二 ob 二 asg 一 8 一 BF(O) 
其 中 ,ao 一 0. 10,ai —0. 10,8—10, 
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摩擦 模型 表示 为 1 (0) —0. 56 十 0. 1sgn(0 )。 参 考 模型 取 90, 十 200 ,十 300, — 50r , 
r 二 sgn(sin(0.05xt)), 即 模型 参数 为 a 二 20,a, = 二 30,b= 二 50。 模 型 及 参考 模型 的 初始 状态 
都 取 [0,0]。 
15 0 16. 62 .25 
soj" MEL Lyapunov 方程 式 (8.7) 得 zl WEN ads 
9 
表示 为 2 一 0. 25e 十 0. 3875£ , 
由 f(6) 的 表达 式 及 仿真 测试 , 可取 FL. —1.0, Bie dil X (8. 9), 自 适应 律 


X. 100,38 A, =1. 5,4, 1.5.2, —1. 5, SE FHABURI PRÉC sat CO FEE UI HR PRG sgn(e) ,边界 
层 厚 度 取 0. 10 ,仿真 结果 如 图 8-1 一 图 8-3 所 示 。 


2 


言 号 
0.25 O id * 则 辅助 信号 


1.3 | 


Ei 
€ 05 
S 
LR 
9 0 
EJ 
S 05 
-l 
-1.5 N 
一 2 
0 50 100 150 
time(s) 
图 8-1 关节 角度 跟踪 
5 
4 
e 3 
-| 
ET 
E 2 
hen 1 
o 
& 0 
a 
S 
-2 
-3 
zl 


o 
un 
© 
S 
四 
ó 


time(s) 
图 8-2 关节 角速度 跟踪 


仿真 结果 表明 ,所 采用 的 鲁 棒 模 型 参考 自 适 应 控制 器 ,不 依赖 于 被 控 对 象 信息 ,适应 未 
知 摩擦 特性 和 参数 的 不 确定 性 ,并 能 保证 对 象 和 模型 的 高 精度 跟踪 。 
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s 


N wm A tA 


control inputu 


0 50 100 150 
time(s) 


8-3 ”关节 控制 输入 


仿真 程序 如 下 : 
(1) Simulink 主 程序 : chap8. 1sim. mdl。 


| 


S-Function2 


chap8 1ctri 


S-Function 


chap8 1plant 


S-Functiont 


(2) 控制 器 S PRX; chap8. lctrl. m, 


function [sys,x0,str,ts] = spacemodel(t, x,u, flag) 
switch flag, 
case 0, 
[sys, x0, str, ts] = mdlInitializeSizes; 
case 1, 
sys = ndlDerivatives(t,x,u); 
case 3, 
Sys = mdlOutputs(t,x,u); 
case (2,4,9) 
sys- []; 
otherwise 
error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0, str, ts] = mdlInitializeSizes 
global p2 p3 
Sizes = simsizes; 


sizes.NumContStates = 3; 
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sizes. NumDiscStates = 
sizes.NumOutputs = 
sizes.NumInputs = 
sizes.DirFeedthrough = 
sizes. NumSampleTimes 


[= 
f 


1 


sys = simsizes(sizes); 
x0 = [0,0,0]; 

str = []; 

ts = [i 


al = 20;a0 = 30;b= 50; 
Am= [0,1; - a0, - al]; 
% eig(Am) 

Q= [15,0;0,15]; 


P = lyap(An',Q); 

p2 = P(1,2); 

p3- P(2,2); 

function sys = mdlDerivatives(t, x,u) 

global p2 p3 

r= sign(sin(0.025 *2* pi*t)); % Square Signal 


thm- u(1); 
dthm = u(2); 
th= u(3); 

dth= u(4); 


e-thm- th; 
de = dthm - dth; 
ep = p2 * e + p3 * de; 


lambda0 = 1. 5;lambda1 = 1.5; lambda2 = 1.5; 
sys(1) = lambda0 * ep * r; % dk0 
sys(2) = lambdal * ep * th; % dk1 
sys(3) = lambda2 * ep * dth; % dk2 
function sys = mdlOutputs(t, x, u) 

global p2 p3 

thm= u(1); 

dthm = u(2); 

th= u(3); 

dth- u(4); 


e-thm- th; 
de = dthm - dth; 
ep = p2 * e + p3 * de; 


r= sign(sin(0.025 * 2 * pi*t)); % Square Signal 


k0 = x(1);k1 = x(2);k27 x(3); 
Fmax = 1.0; 


delta = 0.1; 
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Miri 


kk = 1/delta; 
if abs(ep)» delta 
sats = sign(ep); 
else 
sats = kk * ep; 


end 


* v = Fmax * sign(ep); 

v = Fmax * sats; 

ut = k0 * r+ k1 x th+ k2 * dth+ v; 
sys(1) = ut; 


(3) 参考 模型 S 函数 ; chap8. linput. m, 


function [sys, x0, str,ts] = spacemodel(t,x,u, flag) 
switch flag, 
case 0, 
[sys, x0, str, ts] = mdlInitializeSizes; 
case 1, 
sys = ndlDerivatives(t,x,u); 
case 3, 
sys = mdlOutputs(t, x,u); 
case (2,4,9) 
sys-[]; 
otherwise 
error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0, str,ts] = ndlInitializeSizes 
Sizes = simsizes; 
sizes. NumContStates = 


sizes. NumDiscStates = 


2 
0 
sizes.NumOutputs = 2; 
sizes.NumInputs = 0 
sizes.DirFeedthrough = 1 
sizes.NumSampleTimes = 0 


Sys = simsizes(sizes); 


x0 - [0,0]; 
str = t1 
ts = []; 


function sys = mdlDerivatives(t, x,u) 
al = 20;a0 = 30;b= 50; 
r-sign(sin(0.025*2*pi*t)); % Square Signal 


sys(1) = x(2); 

sys(2) = -al*x(2)-a0*x(1)*b*r; 
function sys = ndlOutputs(t, x, u) 
sys(1) = x(1); 

sys(2) = x(2); 


(4) 被 控 对 象 S 函数 : chap8 lIplant. m, 


function [sys,x0,str,ts] = s function(t, x, u, f lag) 
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switch flag, 
case 0, 

[ sys, x0, str, ts] = ndlInitializeSizes; 
case 1, 

Sys = ndlDerivatives(t,x,u); 
case 3, 

sys = ndlOutputs(t,x,u); 
case (2, 4, 9 ) 

sys = []; 
otherwise 

error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0, str, ts] = mdlInitializeSizes 
sizes - simsizes; 
sizes.NumContStates = 
Sizes.NumDiscStates - 


2 
0 
sizes.NumOutputs = 2; 
sizes. NumInputs = 1 
sizes.DirFeedthrough = 0 
sizes.NumSampleTimes  - 0 

sys = simsizes( sizes); 

x0 = [0;0]; 

str=[]; 

ts=[]; 

function sys = mdlDerivatives(t, x,u) 
alfa0 = 0.10;alfal = 0.10; 


beta = 10; 
ut-7u(1l); 
dth= x(2); 


£-0.5*dth* 0.1* sign(dth); 


sys(1) = x(2); 

sys(2) = beta * ut - beta * f - alfal * x(2) - alfa0 x x(1); 
function sys = mdlOutputs(t, x, u) 

sys(1) = x(1); 

sys(2) = x(2); 


(5) 作 图 程序 : chap8 1plot. m, 


close all; 

figure(1); 

plot(t,x(:,1),'r',t,x(:,3), 'b', '1inewidth',2); 
xlabel('time(s)');ylabel('angle tracking'); 
figure(2); i 
plot(t,x(:,2), 'r',t,x(:,4), 'b', 'linewidth',2); 
xlabel('time(s)');ylabel('angle speed tracking'); 
figure(3); 

plot(t,tol(:,1),'r', 'linewidth',2); 
xlabel('time(s)');ylabel('control input,u'); 
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8.2 二 级 倒立 摆 的 Hecoe 鲁 棒 控 制 


通过 对 文献 L4] 中 的 一 种 二 级 倒立 摆 的 Hee 和 鲁 棒 控制 进行 仿真 分 析 , 研 究 一 种 基于 
LMI 的 Ho 和 鲁 棒 控制 设计 方法 。 
8.2.1 系统 的 描述 
Xi n 阶 广义 受 控 对 象 ,有 
x=Ax+B,w+B,u 
z =C x Di wdD;,u 
y =x (8.11) 
其 中 ,XER”™! SGZCR'US sW ER”! su ER”! ,A B, «B, «Cj D Di; 均 为 相 维 数 的 常 阵 sw 
为 系统 的 建 模 误差 及 外 加 干扰 。 


8.2.2 基于 LMI 的 控制 律 的 设计 
定理 8.27 对 于 式 (8. 11) ,给 定 y 六 0 ,存在 P, —PT770 M P, ,如 果 满 足 不 等 式 ， 


AP,--P,A' -B,P,--PiBi -Y B,Bi (C,P,—- D,P,)* 
«0 (8.12) 
C,P, - D,,P, —]I 
则 Hece 鲁 棒 控 制 的 状态 反馈 控制 器 为 
u —Kx 一 P,Pix (8.13) 


其 中 ,K==[&， ka ks b, ks ks]. 
针对 二 级 倒立 摆 模 型 ,控制 目标 设计 为 : 
(OD x — 0 为 闭环 无 扰动 系统 的 局 部 渐进 稳定 平衡 点 , 即 对 于 初始 状态 x (0), 有 


x(t)>0, 
(20 对 于 任意 扰动 w € L,[0, +2), WRIA x0) — 0, AR RRRA Hè zh dl ERE. 
鲁 棒 性 能 可 表示 为 


| (gta? cd i* CO "at GO HRO EEO HRO c hé OY y | of coi 
(8. 14) 
其 中 ;x 二 [x x 0 Ô, 0, bT zo, 
控制 目标 (1) 反 映 了 倒立 摆 的 基本 控制 要 求 ,x=0 表明 一 级 .二 级 摆 杆 均 处 于 垂直 位 
置 ,小 车 也 处 于 位 置 零点 ,而 且 小 车 及 一 、 二 级 摆 杆 均 没 有 运动 的 趋势 。 


8.2.3 二 级 倒立 摆 系 统 的 描述 


为 了 使 倒立 摆 线 性 化 ,必须 满足 倒立 摆 的 各 级 摆 杆 的 转角 是 小 角度 (小 于 5) ,此 时 
sin0s0,cosgs1。 线 性 化 后 的 二 级 倒立 摆 模 型 为 


X —Ax- B, v - B;u 
(8.15) 
y=x 
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对 于 二 级 倒立 摆 系 统 式 (8. 150 ,引入 控制 输出 为 

Z —C,x 4- Dio - Diu (8. 16) 
由 于 二 级 倒立 摆 系 统 为 六 阶 系统 , 故 取 
[n] 0 0 0 O0 d 


0. a 0.0 0 0 
0. 0 ð 0 0 Ü 
€,—|0 o © mm 0 Dx D=o 0,0 O 0 0 pl Dc. 
0 0 0 0 q; 0 
0 0 0 O0 g 
l0 50 o 909 o l 


其 中 ,gj; 宇 0,j 二 1,2,…,6 为 各 状态 的 加 权 系 数 。 
于 是 式 (8. 16) 变 为 
Z —C,x - Dio - Diu —[qix q;4 q50, qÅ, q:02 qe ov] 
则 


| z 1 =f (qixiG) Hgt A H A Hg 00 + 


q:05 a) + q$05 C) +p u’ (t) }dt 
即 式 (8. 14) 等 价 于 
lzla «xl wl: (8. 17) 
因此 , 求 满足 8.2.2 节 的 设计 目标 (1) 和 (2) 的 控制 器 天 的 问题 就 等 价 于 求 使 闭环 系统 内 部 
稳定 且 满 足 式 (8.17) 的 控制 器 。 
将 式 (8.15) 和 式 (8. 16) 结 合 起 来 ,可 得 用 于 求解 LMI 的 二 级 倒立 摆 系 统 为 
x —Ax +B, vo - B;u 
Z —C,x +D, o - Dyu (8. 18) 
yx 
实现 二 级 倒立 摆 控 制 律 的 设计 ,采用 定理 8. 2 求解 倒立 摆 系 统 式 (8. 18) 的 状态 反馈 控 
制 增益 K 时 ,需要 两 个 LMI, 其 中 ,一 个 LMI 为 式 (8. 12) , 另 一 个 LMI 为 已 ,之 0, 即 
一 P, <0 (8. 19) 
采用 LMI 求 解 工具 箱 一 -YALMIP 工具 箱 求 解 由 式 (8. 12) 和 式 (8. 19) 构 成 的 LMI 
不 等 式 , 从 而 可 以 得 到 K。 


8.2.4 仿真 实例 
二 级 倒立 摆 的 线性 化 模型 为 
TNCS 
yx 
D. d 
A MEC B,)B,—-[B, B,],0; 为 3 阶 零 方 阵 ,1, 为 3 阶 单 
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O 41.9965 9. 3378 7.6261 一 0.0570 | 0.0349|.B,,— 
0 —25.0347 —29.5778 1.0850 0.0675 —0. 0543 

B4-[0 0 0] ,8 一 [一 1.1902 — 55.3119 175. 2019]', B4, = [ 68. 6019 
—115.0316 —16.3660]". 

ER(8. 16) P, po=1,qi =1. 69,9; =1. 0q =0., 01,9, =0. 3,9, —0. 1,94 —0. 01, 

控制 器 增益 的 LMI 求解 程序 为 chap8_2LMI_design. m, 取 y =120, 3R ff LMI 不 等 
式 (8.12) 和 (8. 19), 4$ K=[— 5. 4558 17.4984 50.5226  —4. 7462  — 1. 0376 
—9. 7465]. 

考虑 有 无 干扰 两 种 情况 ,程序 中 分 别 取 M —1 和 M==2。 首 先 考虑 无 干扰 的 情况 ,w==0， 
倒立 摆 的 初始 状态 为 x(0) 二 [0.3 —0.2 0.2 0 0 0]. 仿真 程序 中 , 取 M=1, DE 
结果 如 图 8-4 和 图 8-5 所 示 。 

然后 考虑 有 干扰 的 情况 ,进行 鲁 棒 性 测试 。 根据 互 ce 理 论 ,仿真 中 取 王 扰 为 能 量 有 界 
信号 , 即 取 w 一 sin(0.1)/G 十 1), 取 倒立 摆 的 初始 状态 为 zxC0)=[0 0 0 0 0 0], 仿真 
程序 中 , 取 M 王 2 ,仿真 结果 如 图 8-6 一 图 8-8 所 示 。 其 中 ,图 8-8 为 式 (8. 17) 的 鲁 棒 性 能 验 
证 结果 。 


0 — 3. 7864 0. 2009 
Bt A, = 


—4. 5480 0.0037  —0.0017 
ÅA, = 


yl 
dg eA 


time(s) 


y2 
Ls 


time(s) 


3 
b o 
i ot 
| 


time(s) 


y4 
hou 


time(s) 


5 
Le ie 
ooo 
| 


0 50 100 150 
time(s) 
2 一 一 4 
= 0 
- L 
0 50 100 150 
time(s) 


8-4 ”小 车 、 摆 1 及 摆 2 的 位 置 和 速度 响应 结果 (M=1) 


control input 


-2 
0 50 100 150 
time(s) 


8-5 ”控制 输入 信号 (M=1) 


0 50 100 150 
time(s) 
0.5 
—0.5 
0 50 100 150 
time(s) 
I | 人 一 一 一 
"E 
A 
—0.5 
0 50 100 150 
time(s) 
5 
a 
-5 
0 50 100 150 
time(s) 
2 
-2 
0 50 100 150 
time(s) 
1 
-l 
0 50 100 150 
time(s) 


Bl 8-6 小 车 , 摆 1 及 摆 2 的 位 置 和 速度 响应 结果 (CM 王 2) 
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0.25 
0.2 
0.15 


0.1 


control input 
e 
o 
Un 


50 100 150 
time(s) 


8-7 ”控制 输入 信号 (M=2) 


100 - J 
S gol | 
E 
È 
号 60 E 
E 
E 
c 40 
& 
20 
0 
0 50 100 150 
time(s) 
图 8-8 鲁 棒 性 能 测试 (M 三 2) 
仿真 程序 如 下 : 


(1) 基于 LMI 的 Hce 状 态 反馈 设计 程序 : chap8_2LMI_design. m, 


* H Infinity Controller Design based on LMI for Double Inverted Pendulum 
clear all; 
close all; 


A= [0,0,0,1.0,0,0; 
0,0,0,0,1.0,0; 
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0,0, 0; 0,0, — 1.0; 
0, — 3. 7864,0.2009, — 4.5480,0.0037, - 0.0017; 
0,41.9965,9.3378,7.6261, — 0.0570, 0.0349; 
0, — 25.0347, — 29.5778,1.0850,0.0675, - 0.0543]; 
B1-[0;0;0; —-1.1902; - 55.3119;175. 2019]; 
B2 = [0;0;0;68. 6019; — 115. 0316; - 16.3660]; 


C= eye(6) ; 
D= zeros(6,1); 


9,555559: 9536388599: kkhk kkhk bkk k kkk kkk kkk kkhk kbb bk kk Ekk kkk kkkh 
ql = 1. 69;q2 = 2;q3 = 0. 01;q4 = 0.3;q5 = 0. 1;q6 = 0:01; 

q= [q1, q2, 43, q4, q5,q6]; 

gama = 120; 


C1 = [diag(q);zeros(1,6)]; 
rho= 1; 

D12 = [0;0;0;0;0;0;rho]; 
D11 = zeros(7,1); 


C2 = eye( 6); 

D21 = zeros(6,1); 

555559 kkk b kkrk ktr btts LMI Model Design  56£355:855555:9:9:5: 555596 9c 5 59 95 96 9S 
P1 = sdpvar(6,6); 

P2 7 sdpvar(1,6); 


FAI- [A*P1-*P1*4A'-*B2*P2-4 P2' * B2' * 1/gama^2 * B1 * B1' (C1 * P1 + D12 * P2)';C1 * P1 + D12* 
P2 - eye(7)] ; 


% LMI description 
L1 = set(P1» 0); 
L2 = set(FAI <0); 
LL = L1 + L2; 


solvesdp(LL); 
P1 = double(P1); 
P2 = double(P2); 


K = P2 * inv(P1) 


(2) Simulink 主 程序 : chap8 2sim. mdl, 
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(3) 控制 器 S 函数 : chap8_2ctrl. m, 


function [sys,x0,str,ts] = spacemodel(t, x,u, flag) 
switch flag, 
case 0, 

[sys, x0, str, ts] = ndlInitializeSizes; 
case 3, 

sys = mdlOutputs(t, x, u); 
case (2,4,9) 

sys-[]; 
otherwise 

error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0, str, ts] = mdlInitializeSizes 


Sizes = simsizes; 


oO - 00n^1^ o o 
ws 


sizes. NumContStates 
sizes. NumDiscStates = 
sizes. NumOutputs = 
sizes. NumInputs = 


sizes.DirFeedthrough = 


sizes. NumSampleTimes 


sys = simsizes(sizes); 


x0 = []; 
str = []; 
ts = []; 


function sys = mdlOutputs(t, x, u) 


x-2u; 


K 2[-—5.4558 17.4984 50.5226 — 4.7462 — 1.0376 - 9.7465]; 


ut-K*x; 


sys(1) = ut; 
(4) 被 控 对 象 S 函数 : chap8 2plant. m, 


function [sys,x0, str,ts] = s function(t,x,u, flag) 
Switch flag, 
case 0, 
[sys, x0, str, ts] = ndlInitializeSizes; 
case 1, 
sys = ndlDerivatives(t,x,u); 
case 3, 
sys = mndlOutputs(t, x, u); 
case (2, 4, 9) 
sys = []; 
otherwise 
error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0, str, ts] = mdlInitializeSizes 
global M 
M21; 
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sizes = simsizes; 

sizes.NumContStates = 
sizes.NumDiscStates = 
sizes.NumOutputs = 
sizes. NumInputs - 
sizes.DirFeedthrough  - 
sizes.NumSampleTimes  - 


Q HF* P 0 O OO 
EN 


sys = simsizes( sizes); 
if M== 
x0=[0.3, — 0.2,0.2,0,0,0]; 
elseif M-- 
x0 = zeros(1,6); 
end 
str-[]; 
ts-[]; 
function sys = mdlDerivatives(t, x, u) 
global M 
* Double Link Inverted Pendulum Parameters 
A-[0,0,0,1.0,0,0; 
0,0,0,0, 1.0,0; 
0,0,0,0,0, —1,0; 
0, - 3. 7864, 0. 2009, = 4. 5480,0. 0037, — 0.0017; 
0,41.9965,9.3378,7.6261, — 0.0570,0.0349; 
0, - 25.0347, — 29.5778,1.0850,0.0675, — 0.0543]; 
B1-[0;0;0; — 1.1902; - 55.3119;175.2019]; 
B2 = [0;0;0;68. 6019; — 115. 0316; - 16.3660]; 


ut-u(1); 
ifM-- 
w-0; 
elseif M-- 
w-7sin(0.1*t)/(t*1); 
end 


S= A * x + B1 * w' + B2 * ut; 
for i=1:6 
sys(i)-S(i); 
end 
function sys = mdlOutputs(t,x,u) 
global M 


if M== 
w=0; 
elseif M == 
w-sin(0.1*t)/(t*1); 
end ; 
ql = 1. 69;q2 = 2;qg3 = 0. 01;q4 = 0. 3;q5 = 0. 1;q6 = 0.01; 
rho= 1; 


z= [ql *x(1) q2* x(2) q3* x(3) q4 * x(4) q5 * x(5) q6 * x(6) rho* u(1)]'; 
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zp-22z'*z; 


wp-7w'*w; 


sys(1) = x(1); 
sys(2) = x(2); 
sys(3) = x(3); 
sys(4) = x(4); 
sys(5) = x(5); 
sys(6) = x(6); 
sys(7) = zp; 

sys(8) = wp; 


(5) 作 图 程序 : chap8. 2plot. m, 


close all; 


figure(1); 

subplot(611); 
plot(t,y(:,1), 'r'); 
xlabel('time(s)');ylabel('y1'"); 
subplot(612); 
plot(t,v(:,2), 'z'); 
xlabel('time(s)');ylabel('y2'); 
subplot(613); 
ploE(5,9(5,3), '£')$ 
xlabel('time(s)');ylabel('y3'); 
subplot(614); 

plot(t, y(:,4), 'r'); 
xlabel('time(s)'); ylabel('y4'); 
subplot(615); 

plot(t, y(:,5),, 'r'); 
xlabel('time(s)');ylabel('y5'); 
subplot(616); 

plot(t, y(:,6), 'r'); 

xlabel( 'time(s)');ylabel('y6'); 


figure(2); 
plot(t,u( :,1), 'r'); 
xlabel('time(s)');ylabel('control input'); 


figure(3); 

zp-y(:;7); 

wp- y(:,8); 

gamal - sqrt(zp. /(wp * 0.001)); 
gama = 120; 


plot(t, gama, 'r', t, gamal, 'b'); 
xlabel('time(s)');ylabel('gama and robust performance'); 
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机 械 手 末端 轨迹 及 力 的 
连续 切换 滑 柜 控制 


CHAPTER 9 


9.1 基于 双 曲 正切 函数 切换 的 滑 模 控制 


9.1.1 双 曲 正切 函数 的 特性 


传统 的 滑 模 控制 算法 存在 切换 函数 ,该 函数 会 造成 控制 输入 信和 号 的 拌 振 。 为 了 有 效 地 
消除 抖 振 , 采 用 连续 的 双 曲 正切 函数 代替 切换 函数 是 一 种 有 效 的 方法 。 通 过 双 曲 正切 函 
数 的 陡 度 可 调节 切换 的 程度 。 

双 曲 正切 函数 定义 为 


e —e' 


(9, 1) 


tanh € 
e* 十 gF 
Hp ,e>0,e 的 值 决定 了 双 曲 正切 函数 的 陡 度 。 
由 引 理 9. 1( 见 本 节 附 录 ) 可 知 , 双 曲 正切 函数 满足 ztanh 一 之 0 


9.1.2 仿真 实例 


取 e 王 0.50, 双 曲 正切 函数 和 切换 函数 如 图 9-1 所 示 。 
仿真 程序 : tanh_test. m, 


clear all; 


close all; 
xite- 5.0; 
ts- 0.01; 
for k-21:1:4000; 
S(k) *k*ts- 20; 


Y1(k) = xite * sign(s(k)); 


epc = 0.5; 
y2(k) = xite * tanh(s(k)/epc); 


end 
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— — switch function 
tanh function 


9-] 双 曲 正切 函数 和 切换 函数 


figure(1); 

plot(s, yl, 'r', s, y2, 'k', 'linewidth',2); 
xlabel('s');ylabel('y'); 

legend( Switch function', 'Tanh function'); 


9.1.3 基于 双 曲 正切 函数 的 滑 模 控制 
考虑 如 下 被 控 对 象 


JÖ) 一 xz) 十 dz) (9. 2) 
其 中 ,J 为 转动 惯量 ,0(1) 为 角度 ,u(t) 为 控制 输入 ,d GD) 为 扰动 ,|&G)| 委 D。 
定义 滑 模 函数 为 
s(t) —ceG) -FéGO) 
HB.c0, 
W|e(2—602—0,)0,6 (2-20 (1) 一 6,(1), 其 中 ,04(t) 为 理想 的 角度 信号 。 定 义 
Lyapunov 函数 为 


YY 二 


W| s (22—ce (1)+e(t)=ce (£) F0) —0,G) —ce(O FT Qi 十 d(t)) 一 04(t), 从 而 


V=s =s(cé a +da —8,) 
为 了 保证 闭环 系统 稳定 且 滑 模 函 数 收敛 ,可 采用 以 下 两 种 控制 方法 。 


1. 基于 切换 函数 的 滑 模 控制 


u(t) —J C7 cé + ġa — qs) — DsgnG) (9. 3) 
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si =s (ce ( cé 0 > ns) Dsgn(s) 十 一 d (t) 0 ) 
` ` a ` J | ; J i 


=s(- gs — p DsenG) + 7d) 


eel eps n a eats at tt 
J J 
Jii V —25V ,根据 引 理 9. 3, 可 得 
VO” y) 


n 郧 ,采用 基于 切换 函数 的 控制 律 , 清 模 函数 指数 收 和 收 全 过度 取决 于 VE 
2. 基于 双 曲 正切 的 滑 模 控制 


u(t) =] (— cë + 0a — qs) — Dianh(7-) 
根据 引 理 9. 2,4 s | —stanh( >) pe II D |s| — Dstanb(-) «Due Bü 


— Dstanh(—-) <- D | s | 十 Due 
€ 
从 而 


ss =s(ë +4 Cu -- d») — à.) 


=s (c+ C cë tö, — 9 — Danh (3) - jd) —6,) 


=s- gs— Dinh (7) Jaw) 


= + 了 人 (- Dstanh (3) -- sa 5) 


< 一 入 + 了 (CD | s |+Dpe +sd(t)) 


<— qs’ + n = 一 27V 十 6 


其 中 ,b= n 


即 V 过 一 27V 十 5b。 根据 引 理 9. 3, 可 得 
VG)se wwe) be | e" dr 


0 


—27n(t—t pur 7 271 
= Ig y i ute 
29 
de "vao z- | MNA 
25 
— OCE) + ed -— p Minh. 


29J 


(9. 4) 
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则 

Dpue 

29J 
由 式 (9.5) 可 知 , 滑 模 函 数 s 的 跟踪 误差 e 及 其 变化 率 e 渐进 收敛 ,其 收敛 精度 取决 于 

D,» flle, 


9.1.4 仿真 实例 
基 虑 如 下 被 控 对 象 


(9. 5) 


limV (t) < 


JO) wut) + dy 
取 = 二 10, 理 想 角 度 为 0,(1) 二 sint, 取 d(t) 二 50sint ,被 控 对 象 的 初始 状态 为 L0.5,1.0], 取 
c 二 0.50,7 二 10,D 二 50,e 二 0.02, 分 别 采 用 控制 律 式 (9. 3) 和 式 (9. 4) ,仿真 结果 如 图 9-2 一 
图 9-5 所 示 。 


ideal position signal 
position tracking 


angle response 


angle speed response 


0 5 nm 10 15 


图 9-2 ”基于 切换 函数 的 角度 和 角速度 跟踪 


control input 


10 15 


0 5 


time(s) 


图 9-3 基于 切换 函数 的 控制 输入 
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2 
ideal position signal 
l position tracking 


angle response 
e 


= 
time(s) 
2 
& 
$ 
g 
- 
E 
oD 
8 
0 5 10 15 
time(s) 
图 9-4 基于 双 曲 正切 函数 的 角度 和 角速度 跟踪 
60 
40 
20 
z 
S 0 
E 
8 -20 
-40 
—60 
Bs 5 10 15 
time(s) 
图 9-5 基于 双 曲 正切 函数 的 控制 输入 
仿真 程序 如 下 : 


(1) Simulink 主 程序 : chap9_l1sim. mdl。 


A 
Chap9 1ctri chap9 1plant 
Sine Wave 


S-Functiont XoAVorkspecef 
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(2) 控制 律 SPEC. chap9 lctrl. m, 


function [sys, x0, str,ts] = spacemodel(t, x, u, flag) 
switch flag, 
case 0, 
[sys, x0, str, ts] = ndlInitializeSizes; 
case 3, 
sys = mdlOutputs(t,x,u); 
case (2,4,9) 
sys- []; 
otherwise 
error(['Unhandled flag = ',nun2str(flag)]); 
end 
function [sys, x0, str, ts] = ndlInitializeSizes 


Sizes - simsizes; 


sizes.NumContStates 
sizes.NumDiscStates 


sizes.NumOutputs 


LU 


sizes.NumInputs 
sizes.DirFeedthrough 
sizes.NumSampleTimes 


1 
OPUW PODOD 
EM 


SYS 7 simsizes(sizes); 

x0 = [] 

str = []; 

ts = [E 

function sys = mdlOutputs( t, x, u) 
thd=u(1); 

dthd = cos (七 ) ; 

ddthd = - sin(t); 


th= u(2); 
dth= u(3); 


c-20.5; 
e-th- thd; 
de = dth - dthd; 


s-c*etde; 


J= 10; 
xite - 10; 
D= 50; 
epc = 0.02; 


ut-J*(-c*de-*ddthd- xite * s) =De signia); 
elseif M-- 

ut-J*(-c*de-*ddthd- xite * s) - D* tanh(s/epc); 
end 
sys(1) = ut; 
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(3) 被 控 对 象 S 函数 : chap9_lplant. m, 


function [sys,x0, str, ts] = s function(t, x, u, flag) 
switch flag, 
case 0, 

[sys, x0, str, ts] = ndlInitializeSizes; 
case 1, 

sys = ndlDerivatives(t,x,u); 
case 3, 

sys = ndlOutputs(t, x, u); 
case (2, 4, 9} 

sys = []; 
otherwise 

error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0,str,ts] = mdlInitializeSizes 
Sizes = simsizes; 
sizes. NumContStates - 
sizes.NumDiscStates = 
sizes. NumOutputs = 
sizes. NumInputs = 


sizes.DirFeedthrough 


Oo Oo rn No vx 
€ 


Sizes.NumSampleTimes 
sys = simsizes(sizes); 
x0 - [0.5 1.0]; 
str-[]; 

ts-[] 

function sys = mdlDerivatives(t, x,u) 
J 7» 10; 

dt = 50 * sin(t); 

sys(1) = x(2); 

sys(2) = 1/J * (u+ dt); 

function sys = mdlOutputs(t, x, u) 
sys(1) = x(1); 

sys(2) = x(2); 


(4) 作 图 程序 : chap9 1plot. m, 


close all; 


figure(1); 

subplot(211); 
plot(t,y(:,1),"k',t,y( 1,2), 'r:', 'Tinewidth', 2); 
legend('ideal position signal', "position tracking'); 
xlabel('time(s)');ylabel('angle response'); 
subplot(212); 

plot(t,cos(t), 'k',t,y(:,3), 'r: ', 'linewidth',2); 
legend('Ideal speed signal', 'speed tracking'); 
xlabel('time(s)');ylabel('angle speed response'); 


figure(2); 
plot(t,ut(:,1), 'k', 'linewidth',0.01); 
xlabel('time(s)');ylabel('control input'); 
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附录 


引 理 9. 1 针对 任意 给 定 的 实数 工 , 存 在 下 面 的 不 等 式 


ztanb(7.)— ztanb(7-) =- ze il tanb(7-) |>。 
其 中 ,二 0。 
引 理 9. 1 说 明 如 下 : 根据 双 曲 正切 函数 的 定义 ,有 
ztanh (7) —4 Eus - s = 


由 于 
ew —1220, m 0 


&£*—1 0, aw 


2L 1 E 
M r(e* —1) 220,48 rtanh ——————z (e* —1)220, Bf 
€ 2 
e “十 1 
ztnhb = ztanbž |= æ | uanh 达 | > 
€ € € 


引 理 9.2? 针对 任意 给 定 的 实数 工 , 存 在 下 面 的 不 等 式 
Qi | 一 ztanh( 三 ) < pe, p —0. 2785 
其 中 se 二 0。 


8|389.3. Let f, Vi[0,09) € R,de X V«C—aV-- f .N (24,20, RI 


VE LeV) + Em f(r)dr 


0 


其 中 Q 为 任意 实数 。 
根据 文献 [3], 可 证 明 引 理 9. 3. e (12 V 4-aV — f i o (1) 0. E 


V ——aV 4t f 4-9 
解 方程 可 得 


E 


V(t)-e 


"9" va.) eo roodo [ e* "7? u(r)dr 
WT eo, Vi 人 io 过 0, 则 


一 etl 一 10 


VG) Xe "VG «f e * 7? f(r)dr 


X f—0. RI; V«—aV, Bp 


—a(t—t,) 


VG) xe ” VG) 
A a 为 正 实数 , 则 V(t) 指 数 趋 近 于 零 。 


244 «(|| 机 器 人 控制 系统 的 设计 与 MATLAB 仿 真 ， 基 本 设计 方法 (第 2 版 ) 


9.2 基于 位 置 动 力学 模型 的 机 械 手 末端 轨迹 滑 模 控制 


由 于 机 械 手 通常 是 以 关节 角度 进行 动力 学 建 模 的 ,通过 设计 执行 机 构 施加 的 关节 扭矩 
rz ,可 实现 关节 角度 和 关节 角速度 的 跟踪 。 

然而 ,在 实际 工程 中 ,通常 需要 针对 机 械 手 末端 轨迹 的 控制 ,这 就 需要 建立 工作 空间 关 
节 末 端 节点 直角 坐标 (x ,x,) 的 动力 学 模型 ,并 设计 加 在 关节 末端 节点 的 控制 律 F, ,通过 
F, 与 r 之 间 的 映射 关系 , 求 出 实际 的 关节 扭矩 rr 。 


9.2.1 工作 空间 直角 坐标 与 关节 角 位 置 的 转换 


将 工作 空间 中 的 关节 末端 节点 直角 坐标 (zi,zy) 转 换 为 二 关节 和 角 位 置 (qi,q;) 的 问题 ， 
即 机 器 人 的 逆向 运动 学 问题 。 

根据 图 9-6 ,根据 末 端 端点 在 工作 空间 中 的 位 置 求 关 、 
TREE qi 和 gq, ,文献 [5j 给 出 了 表示 方法 ,但 需要 修正 。 
根据 图 9-6 可 得 末端 在 工作 空间 中 的 位 置 为 


Xi 二 licosgi 十 lzcos(gi 十 qs) 
(9. 6) 
zr; =l sing; t /;sin(q, 十 gs) 
则 
zi + zi = +1} -21ll6sg, 
从 而 可 得 


"EMT EN. 图 9-6 二 自由 度 机 械 手 


qz =arccos( TEN ) (9.7) 


根据 图 9-6, 可 得 


X» T 
a =arctan —, xz; >0 或 a=n+arctan —, xz, <0 
d E 


余弦 定理 是 揭示 三 角形 边 角 关 系 的 重要 定理 ,根据 余弦 定理 ,由 图 9-6 可 得 12 10 
GT xi)—21,4xi--xi cos, Bl 
xi +r +l li 


B — arecos ——— —————— — 
从 而 
o—p, qs 0 
gi 一 (9.8) 
ag. Q0 
f] à 
定义 x 一 [z， chac[n d] dx dg EX J — 25 I 
oq oq 
dx =J * dq 
dr, Iži 
9 1 9 2 —x 
gp. : Es ,表示 机 械 手 末端 端点 速度 与 机 械 臂 关节 角速度 之 间 关 系 的 雅 可 比 


Iq; 9q; 
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AB PE, 
a 2453 : ; Ix d Ix 
H15X C9. 6) n[ 4$ —— — — I, sinq, 一 lysin(qi 十 qs) 4 5— — — l;sin(qi - 452 5— — Lli cosg, 十 
Iq; 9q; qi 
IT 
I, cos(qi qi) ^5. ls cos(qi +q) JU 
[ —l,sin(q;) —/;sin(q; 十 qz)  — l;sin(q, 4- 95) 
Jq = i i ái TU (9.9) 
| Licos(q1) 十 lscos(gi 十 gq;2) lscos(qi 十 qs) 
[—Licos(qgi1)— lscos(qi 十 02) 一 15cos(qi 十 9)| , 
Jig 1 qi 1 i 1 id 
|— lsin(qi) —L;sin(q; 十 gz) 一 上 sin(ql 十 qz) 


[—1,cos(q; +g) — l5cos(q, Hil i 
|— lssin(gi +q) — [;sin(q, + q2) *: 
可 见 ,J(q) 是 由 结构 决定 的 ,假定 它 在 有 界 的 工作 空间 Q 中 是 非 奇 异 的 。 


9.2.2 ”机械手 在 工作 空间 的 建 模 


考虑 一 个 刚性 ”关节 机 械 手 ,其 动态 特性 为 
D(q)j - C(q.d)d —- G(q) ^c (9.10) 
其 中 ,qgER" 是 表示 关节 变量 的 向 量 ,r ER 是 执行 机 构 施 加 的 关节 扭矩 向 量 ,D(q)E 
R” 为 对 称 正 定 惯性 矩阵 ,C(qg,qg)ER"” 为 哥 氏 力 和 离心 力 向 量 ,G (40 € R" 为 重力 
向 量 。 
为 了 实现 末端 位 置 的 控制 ,需要 将 关节 角度 动力 学 方程 转换 为 基于 末端 位 置 的 动力 学 
方程 。 
在 静态 平衡 状态 下 ,传递 到 机 械 手 末 端 力 的 F, 与 关节 力矩 r 之 间 存 在 线性 映射 关系 ， 
通过 虚 功 原理 可 得 吕 
F,—J "(gq (9.11) 
BTi-Jeq.PIq—J x.x—Jq--Jq—JJ x -Jq. Afi 
d—-J^G—J^x 
将 上 式 代 和 人 式 (9. 10) ,可 得 
D(qJJ (š J^? 3x) C(.4)J^ 4 GGD-—c 


即 
Dq) J’ š — Dq J jJ z--C(q.q)J^ X -G(q) —c 
整理 得 
D(QJ^!x4-QGG. -DJJ è +G) =r 
则 


J (DOG ()J + (C(g,g) —DGaJ^jJ^x +G(q)) =J "(qc 
同时 ,考虑 建 模 不 确定 性 ,从 而 得 到 如 下 模型 
D.(qDX --C,(q.q0X -G,(q) -- A(q.d.q) =F, (9. 12) 


其 中 ,D,(q)=J ^ (q0DGOJ ^ (0.C, (Q4) =J 7 (C Q4) DGDJ E (0J (gq) a), 
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G.(q0—J "(gqg)G(q), ll AG 4 4) ll y. 
机 械 手动 态 方程 具有 下 面 的 特性 : 
CD 惯性 矩阵 D, (gq) 对 称 正定 。 


(2) ERE D, (q0—2C, (q ,9 ) 是 斜 对 称 的 。 | 
9.2.3 滑 模 控制 器 的 设计 


设 xs(C) 是 在 工作 空间 中 的 理想 轨迹 , 则 xu,() 和 xu() 分 别 是 理想 的 速度 和 加 速度 。 
定义 
[As =y (t) =x (t) 
x. (t)=xļž,(t)+4e(t) 
s(t) =x, (t)—ž(t)=6(t)+4Ae(t) 
其 中 ,A 为 正定 矩阵 。 
设计 一 种 具有 光滑 双 曲 正切 切换 的 滑 模 控 制 絮 为 
F, — D, (DX, -- C, (gx, +G, (q) + Ks + p tanh 5 (9.13) 


H'B.K0,670, 
将 控制 律 式 (9. 134 AR C9. 12) ,得 
D, (q)¥ --C,(q.qOX - G, (q4) -- ACq.q.qD 


=D, GDX, +C, (q.d)X, - G, (q) -- Ks 十 ytanh > 


K x—x,—sx-—x,—s RAER 
D, (q0$ +C, (q.d)s +Ks 十 ?tanh Š — Alq 4d) —0 
由 于 D, (gq) 为 对 称 正 定 , 则 可 定义 Lyapunov 函数 
V=7s"D,(q)s 
则 
V —s'D,$ + 75 D,s 
由 于 和 矩阵 DD, (q) 一 2C, (q ,9 ) 是 斜 对 称 的 , 则 8T(D — 2C, )s 0,8757 D,s —sTC,s, 
代入 上 式 得 
V —s'D,$ --s'C,s =s" (D.s --C,s) ^s" (—Ks — 7 tanh 十 A(qg,qg,9)) 
根据 引 理 9. 1 和 引 理 9. 2, 有 
s" (7 ytanh + ACa d 2) — — ys tanh cs" A(g «d d) 


«— s ll 十 7pe 十 SA(Gq 49) X que 
其 中 ,一 0. 2785. 
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于 是 


V «—s'Ks 4- que S— À min (Os! s + que 


ek ET AV t 
DY 2^ Ps bep 2 


(K) 
RB sA ma DaF AÀ min KODA D, 和 K 的 最 大 特征 值 ,4 — e Te. 


RE V<—2AV+b, E 3| 9. 377 ,可 得 


es 1$.) 


VG») x "VG, n e^ dr 


外 ta) 214 


ó e“ —e ") 


—LA 1.) —2AG-—t,) 


? v, x nnl" 
则 
limV G) < r^ 


根据 上 式 ,跟踪 误差 和 误差 导数 渐进 收敛 ,收敛 精度 取决 于 e、X Fm Bl e 越 小 ,K RAK, A 
越 小 ,收敛 效果 就 越 好 。 


9.2.4 仿真 实例 


考虑 平面 两 关节 机 械 手 ,机 器 人 的 动力 学 方程 为 
D(q)q--C(q.q0d - G(q) —c 
其 中 ， 
m;--m;--2m4cosq; m;--m;cosq; 
也 (9 ) 一 | | 


m; +m cosq: m» 
Chad Mo vina — m;(q, 十 gz)sing， 
m 3q,sinq; 0.0 


TAN men- +m; gcos(q, in 
m;gcos(q, 4- q;) 
ERF m, fii M—P-- p,L 给 出 ,有 
M-—[m, m, m, m, m;] 
P=[p: b: bs b. bil 
Left WU h, 4 Al 
其 中 ,p, 为 负载 ,和 AL 分 别 为 关节 1 和 关节 2 的 长 度 ,P 是 机 器 人 自身 的 参数 向 量 。 机 
械 力 臂 实际 参数 为 p, 二 0. 50,P=[1.66 0.42 0.63 3.75 1.25]',4,—1,—1. 
在 笛 卡 儿 空间 中 的 理想 跟踪 轨迹 取 zu = cost ,zu 一 sint, 该 轨迹 为 一 个 半径 为 1. 0, 圆 
心 在 (ziyzz) 王 (1.0,1.0) 的 圆 。 初 始 条 件 为 x(0) 王 [1.0 1.0],z(0) 王 [0.0 0.0]. 
由 于 跟踪 轨迹 为 工作 空间 中 的 直角 坐标 ,而 不 是 关节 空间 中 的 角 位 置 ,应 按 式 (9.7) 和 
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式 (9.8) 将 工作 空间 中 的 关节 末端 直角 坐标 (zi ,zy ) 转 换 为 关节 角 位 置 Cqo ,gq,)。 
按 工作 空间 模型 式 (9. 12) 实 现 被 控 对 象 的 描述 ,考虑 A(g ,gq d) = lsin , 滑 模 控制 器 


Nik C. 13) ,并 通过 式 (9. 11) 可 转换 为 实际 控制 器 ,控制 器 的 增益 选 为 玉 一 » M "T 


15 0 E 
| s ,7 二 12,e 王 0.10。 仿真 结果 如 图 9-7 — F8 9-10 Brzn 


A2 
8 ideal x 
s 1 — — —* practical x 
oD 
B 
z0 
£ 
ga 
& -2 
time(s) 
- ideal y 
S — — —* practical y 
bb 
= 
E 
E 
E 
[s] 
治 
$ 
A 


time(s) 


图 9-7 末 关 节 节 点 的 位 置 跟 踪 


ideal dx 
一 一 一 ' practical dx 


velocity tracking of x axis 


time(s) 


— —— ideal dy 
= = = practical dy | 


N 


© 


velocity tracking of y axis 
| 
N 


a 上 


time(s) 


9-83 ” 末 关 节 节 点 的 速度 跟踪 
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Fx of first link 
— — —' Fx of second link 


conrol input Fx1 and Fx2 


tol of first link 
— — — tol of second link | 


conrol input toll and tol2 


time(s) 


图 9-9 控制 输入 ,和 tr 


ideal trajectory | 


practical trajectory 


一 1.5 al -0.5 0 0.5 1 1.5 
x 
图 9-10 轨迹 跟踪 效果 


仿真 程序 如 下 : 
(1) Simulink 主 程序 : chap9_2sim. mdl。 


chap9_2input 


S-Function2 
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(2) 输入 指令 S 函数 : chap9_2input. m, 


function [sys,x0, str,ts] = spacemodel(t, x,u, flag) 
switch flag, 
case 0, 

[sys, x0, str, ts] = mdlInitializeSizes; 
case 1, 

sys = ndlDerivatives(t,x,u); 
case 3, 

sys = ndlOutputs(t,x,u); 
case (2,4,9) 

sys-[]; 
otherwise 

error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0, str,ts] = mdlInitializeSizes 
sizes - simsizes; 
sizes. NumContStates = 
sizes. NumDiscStates = 


sizes. NumInputs = 
sizes.DirFeedthrough = 


0 
0 
sizes.NumOutputs - 6; 
0 
1 
sizes.NumSampleTimes 1 


Sys = simsizes(sizes); 


x0 - []; 
str = []; 
ts = [00]; 


function sys = mdlOutputs(t, x, u) 
xdl = cos(t); 

d xdi = - sin(t); 
dd xdl- - cos(t); 
xd2 = sin(t); 

d xd2 = cos(t); 

dd xd2 = - sin(t); 
sys(1) = xdl; 
sys(2) = d xdl; 
Sys(3) = dd xd1; 
sys(4) = xd2; 
sys(5) ^ d xd2; 
sys(6) = dd xd2; 


(3) 控制 器 及 被 控 对 象 S IR: chap9 2system. m, 


function [sys,x0, str,ts] = s function(t, x, u, f lag) 
switch flag, 
case 0, 
[sys, x0, str, ts] = mdlInitializeSizes; 
case 1, 
sys = ndlDerivatives(t,x,u); 
case 3, 
sys = ndlOutputs(t,x,u); 
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case (2, 4, 9 ) 
sys = []; 
otherwise 
error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0, str, ts] = ndlInitializeSizes 
global J Fx 
Sizes = simsizes; 
sizes.NumContStates = 
sizes.NumDiscStates = 
sizes.NumOutputs = 
sizes. NumInputs = 
sizes.DirFeedthrough = 
sizes. NumSampleTimes 


[= 


sys = simsizes(sizes); 

x0-[101 0]; 

str-[]; 

ts-[]; 
J=0;Dx= 0;Cx= 0;Gx= 0;Fx= [0 0]; 
function sys = mdlDerivatives(t,x,u) 
global J Fx 

xdl = u(1); 

d xdl = u(2); 

dd xdl = u(3); 

xd2 = u(4); 

d xd2 = u(5); 

dd xd2 = u(6); 


11721;1221; 
P= [1.66 0.42 0.63 3.75 1.25]; 
g79.8; 


L-[11^212"2 11 * 12 4 12]; 
pl=0.5; 


M-P*pl*L; 

Q= (x(1)^2*x(3)^2-1172- 172)/(2* 11 * 12); 
q2 = acos(Q) ; 

dq2 = - 1/sqrt(1- Q^2); 


A= x(3)/x(1); 
pl = atan(A); 
d pl 7 1/(1* ^2); 


B-sqrt(x(1)^2* x(3)^2* 1172- 1222)/(2*11* sqrt(x(1)^2 + x(3)^2)); 
p2 = acos(B); 
d p2- - 1/sqrt(1- B^2); 


if q2»0 
ql = pl - p2; 
dql =d pi-d p2; 
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else 
q1 = p1 + p2; 
dql =d pl+d p2; 
end 
J-[-sin(gl)- sin(gl +q2) - sin(q1 + q2); 
cos(ql) + cos(q1 + q2) cos(ql+q2)]:; 
d_J = [ - dq1 * cos(q1) - (dq1 + dq2) * cos(q1 +q2) - (dql + dq2) * cos(q1 + q2); 
- dgl * sin(q1) - (dql + dq2) * sin(ql + q2) - (dal + dq2) * sin(q1 + q2)]; 


D-[M(1) * (2) *2* M(3) * cos(q2) M(2) +M(3) * cos(q2); 
M(2) * M(3) * cos(q2) M(2)]; 
C-[-M(3)*dqg2* sin(q2) - M(3) * (dql + dq2) * sin(q2); 
M(3) x dql x sin(q2) 0]; 
G- [M(4) * g* cos(q1) * M(5) * g* cos(ql + q2); 
M(5) * g * cos(q1 + q2)]; 


Dx= (inv(J))'* D* inv(J); 
Cx= (inv(J))'* (C- D* inv(J) *d J) * inv(J); 
Gx= (inv(J))'* G; 


el = xdl - x(1); 

e2 = xd2 - x(3); 
del = d_xd1 - x(2); 
de2 = d_xd2 - x(4); 
e-[el;e2]; 

de = [de1;de2]; 


Hur = 15 * eye(2) ; 
r= de + Hur * e; 


dxd = [d xdi;d xd2]; 
dxr = dxd * Hur * e; 

ddxd = [dd xdl;dd xd2]; 
ddxr = ddxd + Hur * de; 


K= 30 * eye(2); 

epc = 0.10; 

xite = 12; 

Fx = Dx * ddxr + Cx * dxr + Gx + K * r + xite * tanh(r/epc); 


Delta = 10 * sin(t); 
dx= [x(2) ;x(4)]; 
S= inv(Dx) * (Fx— Cx * dx - Gx- Delta); 


sys(1) = x(2); 

sys(2) = S(1); 

sys(3) = x(4); 

sys(4) = S(2); 

function sys = mdlOutputs(t, x, u) 
global J Fx 
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tol = J' * Fx; 


sys(1) = x(1); 
sys(2) = x(2); 
sys(3) = x(3); 
sys(4) = x(4); 
sys(5:6) = Fx(1:2); 
Sys(7:8) = tol(1:2); 


(4) 作 图 程序 : chap9. 2plot. m, 


close all; 


figure(1); 

subplot(211); 

plot(t,xd(:, 1), 'r',t,x(:,1), 'b-- ', 'linewidth',2); 
xlabel('time(s)');ylabel('position tracking of x axis'); 
legend('ideal x', 'practical x'); 

subplot(212); 

plot(t,xd(:,4), 'r',t,x(:,3), 'b-- ', 'linewidth',2); 
xlabel('time(s)');ylabel( position tracking of y axis'); 
legend('ideal y', 'practical y'); 


figure(2); 

subplot(211); 

plot(t, xd(:,2), 'r',t,x(:,2), b-- ', 'linewidth',2); 
xlabel('time(s)');ylabel('velocity tracking of x axis'); 
legend('ideal dx', 'practical dx'); 

subplot(212); 

plot(t,xd(:,5), 'r',t,x(:,4), 'b-- ', 'linewidth',2); 
xlabel('time(s)');ylabel(' velocity tracking of y axis'); 
legend('ideal dy', 'practical dy'); 


figure(3); 

subplot(211); 

plob(E, x(:,5) , 'r',£, x( 1,6), 'b.—— ", "Tinegidth", 2); 
xlabel('time(s)'); ylabel('conrol input Fx1 and Fx2'); 
legend('Fx of first link', 'Fx of second link'); 
subplot(212); 

plot(t,x(:,7), 'r',t,x(:,8), 'b—- ', '1linewidth',2); 
xlabel('time(s)');ylabel('conrol input toll and tol2'); 
legend('tol of first link', 'tol of second link'); 


figure(4); 

plot(xd(:,1),xd(:,4), 'r', linewidth',2); 
hold on; 

plot(x(:,1),x(:,3), 'b-- ', 'linewidth',1); 
xlabel('x');ylabel('y'); 


legend('ideal trajectory', 'practical trajectory'); 


I> 255 
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9.3 基于 角度 动力 学 模型 的 机 械 手 末端 轨迹 滑 模 控制 


由 于 机 械 手 通常 是 以 关节 角度 进行 动力 学 建 模 的 ,通过 设计 执行 机 构 施 加 的 关节 扭矩 
rt ,实现 关节 角度 和 关节 角速度 的 跟 踩 。 

然而 ,在 实际 工程 中 ,通常 需要 针对 机 械 手 末端 轨迹 的 控制 ,这 就 需要 建立 工作 空间 关 
节 末 端 节点 直角 坐标 (x1 ,x,) 与 关节 角度 之 间 的 关系 。 本 节 讨 论 针对 角度 动力 学 模型 设 
计 控 制 器 ,实现 末端 节点 的 位 置 跟踪 。 


9.3.1 机 械 手 在 工作 空间 的 建 模 


考虑 一 个 刚性 ”关节 机 械 手 ,其 动态 特性 为 
Dq -- C(q.q0d - G(q) —c—d (9. 14) 
HP, q ER 是 表示 关节 变量 的 向 量 ,r ER 是 执行 机 构 施 加 的 关节 扭矩 向 量 ,dER"* 为 加 
在 控制 输入 上 的 扰动 ,d= 二 [di,d,],D(q)E€ER”*" 为 对 称 正定 惯性 矩阵,C(q,q)ER"” ”为 
哥 氏 力 和 离心 力 向 量 ,G(qg)ER" 为 重力 向 量 。 
与 9. 2 节 中 针对 位 置 模型 式 (9. 12) 设 计 控 制 器 不 同 , 本 节 讨 论 通 过 对 关节 角度 的 控制 
实现 末端 位 置 的 跟踪 。 


9.3.2 工作 空间 直角 坐标 与 关节 角 位 置 的 转换 


针对 工作 空间 中 机 械 手 末 端 位 置 轨迹 控制 问题 ,由 于 跟踪 轨迹 为 工作 空间 中 的 直角 坐 
标 , 而 不 是 关节 空间 中 的 角 位 置 , 因 此 需要 将 工作 空间 中 的 给 定 的 关节 末端 节点 直角 坐标 
(x1,Xs) 转 换 为 相应 的 关节 角 位 置 (g, q) ,解决 机 器 人 的 道 向 运动 学 问题 。 末 端 节点 直角 
坐标 (x ,xs) 和 相应 关节 角 位 置 (gq, ,gs) 的 转换 采用 9. 2 节 中 式 (9.6) 一 式 (9.8) 的 形式 。 


9.3.3 滑 模 控制 器 的 设计 
针对 式 (9. 14) 8 25 q (G2 —q G0 qu 00 ,定义 


d.—4.—AÀ4d. d. —4d.—Ad (9.15) 
à 6 
其 中 ,A = A 290,4 1,8. 
0 A; 
滑 模 函数 为 
s—q--Aq (9. 16) 
设计 控制 器 为 
t — D(q0Q, - C(q .q0q, + G(q) — Kns 一 7TSgns (9. 17) 
ka 0 
其 中 ,Kb== 0 k ska >00 >max( |d|; ld:|). 
d2 


由 于 H 为 正定 阵 , 设 计 Lyapunov 函数 为 


NR 
V Ds 
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则 有 


à E- undis qx és ls 
V =s'Ds Vs Ds =s" (Dë — Dd) - 7s Ds 


: x ^ l re 
=s" (r —d — Cq G — Dį,) t 75 Ds 


—s'(r—d—Cí(s4- 0 —G—Dà4Q) + s Ds 
将 控制 律 式 (9.17) 代 人 上 式 , 得 


V —5s' Gd, -- CQ, +G — Kos 一 7sgns — d 


P e 1 。 
Cs ^4) —G —D4) - s Ds 
$ . l a 
一 8 (7 Kos — gsgns — Cs — d) + s Ds 


3 a l uis 
——s'Kpys —7 ls I —s'd-F 4s (ID — 2O)s 


Y €—s3 Kg E uV 


. 和 À pmax 分 别 为 K, fil D 的 最 小 和 最 大 特征 值 。 


采用 不 等 式 求解 引 理 9. 3 ,不 等 式 方程 V 受 一 wyV 的 解 为 
VG) Se VO) (9. 18) 
从 而 可 得 , 当 上 -co 时, 滑 模 函 数 y AE AE LI 9 一 0,9 一 0 且 指 数 收敛 ,收敛 精度 取决 于 参 
数 u fü. 
9.3.4 仿真 实例 


考虑 平面 两 关节 机 械 手 ,机 器 人 的 动力 学 方程 为 
D(q)0q —- C(q.404 +G) —c 


其 中 ， 
mi 十 ms 十 2mscosqs ms--m;cosqs 
D(q) = 
m + mMm,cosqe m» 
, —mi;q,sinq; — m4(q, 十 qz)sing 
Ga 有) 三 i v . 
maqising; 0.0 


m,gcosq, ^ m;gcos(q, o qs) 
G(q) 一 
m;gcos(q, 4-q;) 
式 中 ,m; fii M—P- pL 给 出 ,有 


M=[m: m, m; m, m;] 


P=[p: b: b: ba bis] 
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BL 
其 中 ,p, 为 负载 ,1， LL, 分别 为 关节 1 和 关节 2 的 长 度 ,P 是 机 器 人 自身 的 参数 向 量 。 机 
械 力 臂 实际 参数 为 p,—0.50,P—[1.66 0.42 0.63 3.75 1.25]'.1 —1, 0.25. 
第 卡 儿 坐标 平面 内 的 期 望 轨迹 如 表 9-1 所 未 。 


表 9-1 笛 卡 儿 坐 标 平面 内 的 期 望 轨迹 


x h HH B LE y 轴 期 望 轨迹 
1 x 1 
Za 一 一 二 cos 7! 34— 75 l cosnt) 
T 
x Ds . ; 
i Su y jg SIBRE 
= "p 
区 EI 3 
Ta =g ot Ya =p cosnt 


针对 工作 空间 中 机 械 手 末端 的 轨迹 控制 问题 ,由 于 被 控 对 象 给 定理 想 位 置 为 坐标 值 , 而 
被 控 对 象 模 型 采用 的 是 角度 值 , 为 此 必须 将 理想 坐标 值 转化 为 理想 角度 值 。 根 据 式 (9. 7)， 
可 得 


+y =l; 
qa = arccos (5322) (9.19) 
142 
zi tyi +l 
Yt amata a a, — nt arctan 23 (x ,«0) , B, — arecos RE € 
Tà Tu 21,4] x3 Fyi 
据 式 (9. 8) ,可 得 
aa — Bos qd 2d 之 0 
qii = (9. 20) 
ao 十 po， qa S0 


"UO 
HICUTROÉLRURE ROC q Ma PURGE qu CO — f MEL 
dza 


中 ,需要 对 gu(z) 求 一 次 和 二 次 导数 ,而 该 求 导 过 程 过 于 复杂 ,为 了 简单 起 见 ,可 采用 如 下 三 
阶 积分 链 式微 分 器 实现 44(7) 和 全 (CD)59 : 


Tı ST: 

PA TA (9. 21) 
k 5 k; 

Ža 一 一 一 (zi 一 04) 一 一 之 2 一 二 
E e? E 


微分 器 的 输出 m. 和 xz; 为 94(t) 和 g4(t)。 为 了 抑制 微分 器 中 的 峰值 现象 ,在 初始 时 刻 
0 和 :和 1.0 时 , 取 


g———(1-—e&6*) 


按 式 (9. 19) RI SX C9. 20) 将 工作 空间 中 的 关节 末端 直角 坐标 (xz,,ya) 转 换 为 关节 角 位 置 
(qi sda) o 采用 式 (9.6) 实 现 末 端 实际 坐标 点 坐标 (x ,Xs ) 的 描述 , 滑 模 控制 器 取 式 (9. 17) , 控 


130 0 50 0 
制 器 的 增益 选 为 Ko 一 | |4=| | 仿 丰 结果 如 图 9-11~ 图 9-15 所 示 ， 
0 130 0 50 
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图 9-15 机械 臂 位 姿 运 动 轨 迹 


仿真 程序 如 下 : 
(1) Simulink 主 程序 : chap9_3sim. mdl( 包 括 以 下 两 个 部 分 ) 。 


m 


hap9_3ctri 
S-Function3 


Simulink 主 程序 下 的 输入 模块 
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(2) 输入 指令 S PR: chap9_3input. m, 


function [sys,x0, str,ts] = inputsignal(t,x,u, flag) 
Switch flag, 
case 0, 3 
[sys, x0, str, ts] = mdlInitializeSizes; 
case 3, 
sys = ndlOutputs(t,x,u); 
case {2,4,9} 
sys=[]; 
otherwise 
error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys,x0, str, ts] = ndlInitializeSizes 


sizes = simsizes; 


sizes. NumOutputs 


sizes. NumInputs 


sizes. DirFeedthrough 


LU 
H oo ts» 


sizes. NumSampleTimes 


SyS = simsizes(sizes); 


x0 = []; 
str = []; 
ts = [00]; 


function sys = mdlOutputs(t, x, u) 
11= 0.25;12= 0.25; 


xd= —0.25 * cos(pi/2 * t); 
yd=0.2x (1-cos(pi*t)); 
dxd = 0.25 * pi/2 * sin(pi/2 * t); 
dyd= 0.2 * pix sin(pi * t); 


if(xd»- 0) 
alfa0 = atan(yd/xd); 
else 
alfa0 = pi + atan(yd/xd) ; 
end 
beta0 = acos( (xd^2 + yd^2 + 11^2 — 12^2) /(2 * 11 * sqrt(xd^2 + yd^2))); 
qd2 = - acos((xd^2 + yd^2 - (11^2 + 12^2))/(2* 11 * 12)); 


if(qd2» 0) 
qd1 = alfa0 - beta0; 
else 
qdi = alfa0 + beta0; 
end 
sys(1) = adl; 
sys(2) = qd2; 
sys(3) = xd; 
sys(4) = yd; 
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(3) 控制 器 S 函数 : chap9_3ctrl. m, 


function [sys,x0, str,ts] = spacemodel(t, x,u, flag) 
switch flag, 
case 0, 
[sys, x0, str, ts] = mndlInitializeSizes; 
case 1, 
sys = ndlDerivatives(t,x,u); 
case 3, 
sys = ndlOutputs(t, x,u); 
case (2,4,9) 
sys- []; 
otherwise 
error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0, str, ts] = mdlInitializeSizes 


sizes - simsizes; 


LU 


sizes. NumContStates 0; 
sizes.NumDiscStates -0; 
sizes.NumOutputs = 2; 
sizes.NumInputs - 10; 
sizes.DirFeedthrough = 1; 
sizes.NumSampleTimes = 1; 
Sys 7 simsizes(sizes); 

x0 = []; 

str = []; 

ts = [00]; 

function sys = ndlOutputs(t, x, u) 
qdl = u(1); 

dqd1 = u(2); 

ddqd1 = u(3); 

qd2 = u(4); 

dad2 = u(5); 

ddad2 = u(6); 


ql = u(7);dql = u(8); 
q2 = u(9);dq2 = u(10); 
dq = [dal dq2]'; 


11 = 0.25;12 = 0.25; 

P= [1.66 0.42 0.63 3.75 1.25]; 
g=9.8; 

L-[11^2 12^2 11 * 12 11 12]; 
pl= 0.5; 

M-P*pl*I; 


D-[M(1) * (2) * 2* M(3) * cos(q2) M(2) + M(3) * cos(q2); 
M(2) + M(3) * cos(q2) M(2)]; 

C-[-M(3) *dq2* sin(q2) —-M(3) * (dal + dq2) * sin(q2); 
M(3) * dgl * sin(q2) 0]; 

G- [M(4) *g* cos(q1) * M5) *g*cos(ql + q2); 
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M(5) * g * cos(q1 + q2)]; 


el = ql - adl; 

e2 7 q2 - qd2; 

del = dql - dqd1; : 
de2 = dq2 - dqd2; 

e= [el;e2]; 

de = [de1;de2]; 


Hur = 50 * eye(2) ; 
s = de + Hur * e; 


dad = [dqd1 ;dqd2]; 
dar = dad - Hur * e; 
ddad = [ddad1 ;ddqd2 ] ; 
ddqr = ddad - Hur * de; 


KD = 130 * eye(2); 
tol = D * ddqr*C*dqrtG-KD*s; 


sys(1:2) = tol(1:2); 
(D 微分 器 S PRÉC: chap9. 3TD. m, 


function [sys,x0, str,ts] = spacemodel(t, x,u, flag) 
switch flag, 
case 0, 
[sys, x0, str, ts] = ndlInitializeSizes; 
case 1, 
sys = ndlDerivatives(t, x,u); 
case 3, 
sys = ndlOutputs(t, x,u); 
case (2,4,9) 
sys - []; 
otherwise 
error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0, str,ts] = ndlInitializeSizes 


sizes = simsizes; 


sizes.NumContStates 


sizes.NumDiscStates 
sizes.NumOutputs 


sizes.NumInputs 
sizes.DirFeedthrough 
sizes. NumSampleTimes 


SyS 7 simsizes(sizes); 


1 
户口 
` 


x0 = [000]; 
str = []; 
ts = [00]; 


function sys = mdlDerivatives(t, x,u) 
v=u(1); 
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al-79;bl-27;cl- 27; 
kexi = 0.01; 
if t<=1 
kexi - 1/(100 * (1— exp( -2* t))); 
end 
sys(1) = x(2); 
sys(2) = x(3); 
sys(3) = -al * (x(1) - v)/kexi^3 - b1 * x(2)/kexi^2 - c1 * x(3)/kexi; 
function sys = ndlOutputs(t, x, u) 
v-u(1); 
sys(1)- v; 
sys(2) = x(2); 
sys(3) = x(3); 


(5) 被 控 对 象 S PREX: chap9 3plant. m, 


function [sys,x0, str,ts] = s function(t, x, u, f lag) 
switch flag, 
case 0, 

[sys, x0, str, ts] = ndlInitializeSizes; 
case 1, 

sys = ndlDerivatives(t, x,u); 
case 3, 

Sys = mdlOutputs(t, x, u); 
case (2, 4, 9 } 

sys * []; 
otherwise 

error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0, str, ts] = ndlInitializeSizes 
Sizes = simsizes; 
sizes.NumContStates = 
sizes.NumDiscStates = 
Sizes.NumOutputs = 


sizes.DirFeedthrough = 
sizes.NumSampleTimes  - 
Sys = simsizes( sizes); 
x0-[1010]; 

str-[]; 

ts- [I]; 

function sys = mdlDerivatives(t, x, u) 
11720.25;127 0.25; 

P-[1.66 0.42 0.63 3.75 1.25]; 
g79.8; 

L-[11^2 12^2 11 * 12 11 12]; 
pl20.5; 

M=P+pl* L; 


4 
0 
4 
sizes.NumInputs 22; 
1 
0 


qi = x(1);dq1 = x(1); 
q2 = x(3);dq2 = x(4); 
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dq= [dql dq2]'; 


D= [M(1) +M(2) * 2 * M(3) * cos(q2) M(2) +M(3) * cos(q2); 
M(2) * M(3) * cos(q2) M(2)]; 
C-2[-M(3)*dqg2* sin(q2) -M(3) * (dal + dq2) * sin(a2); 
M(3) * dql * sin(q2) 0]; 
G- [M(4) * gx cos(q1) * M(5) * gx cos(al * q2); 
M(5) * gx* cos(ql + q2)]; 


tol = [u(1) u(2)]'; 
S= inv(D) * (tol- Cx dq- G); 


sys(1) = x(2); 
sys(2) = S(1); 
sys(3) = x(4); 
sys(4) - S(2); 
function sys = mdlOutputs(t, x, u) 


sys(1) = x(1); 
sys(2) = x(2); 
sys(3) = x(3); 
sys(4) = x(4); 


(6) 作 图 程序 : chap9. 3plot. m, 


close all; 


figure(1); 

subplot(211); 

plot(t,q(:,1), 'r',t,a( :,7), 'b-- ', 'Linewidth',2); 
xlabel('time(s)');ylabel('angle tracking of first link'); 
subplot(212); 

plot(t,q(:,4), 'r',t,q( 2,9), 'b-- *, '1inewidth',2); 
xlabel('time(s)');ylabel('angle tracking of second link'); 


figure(2); 

subplot(211); 

plot(t,q( 5,2), 'x', £, q( :,8);, 'b-— *, linewidth 2) ; 
xlabel('time(s)');ylabel('angle speed tracking of first link'); 
legend('ideal dx', "practical dx'); 

subplot(212); 

plot(t,q(:,5), 'r',t,q(:,10), 'b-- ', 'linewidth',2); 
xlabel('time(s)');ylabel('angle speed tracking of second link'); 


figure(3); 

plot(t,tol(:,1), 'r', linewidth',2); 

hold on; 

plot(t,tol(:,2), 'b-- ', 'linewidth',1); 
xlabel('x');ylabel('control input'); 
legend('tol of first link','tol of second link'); 
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figure(4); 

plot(pd(:,1),pd(:,2), 'r'); 

11 = 0. 25;12 = 0.25; 

q1-39q(:,7); 

q2-79(:,9); 

x-1ll*cos(q 1)*12*cos(q 1 * q 2); 
y7li*sin(q1)*12*sin(q 1*q 2); 


hold on; 
plot(x(:),v(:), 'b'); 
xlabel('x'); ylabel('y'); 
grid on 


legend('ideal trajectory', practical trajectory'); 


9.4 工作 空间 中 双关 节 机 械 手 末端 的 阻抗 滑 模 控制 


9.4.1 问题 的 提出 


工业 机 器 人 可 以 完成 的 任务 可 以 分 为 两 类 :一 类 是 非 接触 性 作业 , 即 机 器 人 在 自由 空间 
中 搬运 .操作 目标 物 等 任务 ,对 于 这 一 类 作业 ,仅仅 运用 位 置 控制 便 可 以 胜任 ; 另 一 类 是 接 
触 性 作业 ,如 抛光 .打磨 等 ,对 于 这 一 类 任务 ,单纯 的 位 置 控制 已 经 不 能 胜任 了 ,因为 在 这 类 
任务 中 对 接触 力 的 大 小 是 有 要 求 的 ,并 且 机 器 人 末端 微小 的 位 置 偏差 就 可 能 导致 巨大 的 接 
触 力 ,会 对 机 器 人 和 目标 物 造 成 损害 ,所 以 必须 添加 接触 力 的 控制 功能 来 提高 机 器 人 的 有 效 
作业 精度 。 

Hongan 提出 机 器 人 的 阻抗 控制 方法 ,机 器 人 阻抗 控制 就 是 间接 地 控制 机 器 人 和 环 
境 间 的 作用 力 , 其 设计 思想 是 建立 机 器 人 末端 作用 力 与 其 位 置 之 间 的 动态 关系 ,通过 控制 机 
器 人 位 移 而 达到 控制 末端 作用 力 的 目的 ,保证 了 机 器 人 在 受 约束 的 方向 保持 期 望 的 接触 力 。 
自 阻 抗 控 制 概念 被 提出 以 来 ,涌现 出 很 多 不 同 的 具体 应 用 方法 。 由 于 工业 机 器 人 都 匹配 有 
高 性 能 的 位 置 控 制 器 ,所 以 基于 位 置 的 阻抗 控制 策略 得 到 了 广泛 的 应 用 。 

带 有 阻力 约束 的 双关 节 机 械 手 示意 图 如 图 9-16 MRY ,机 械 手 末端 接触 到 障碍 物 后 ， 
IEEE x, 的 方向 滑 下 ,然后 继续 跟踪 指令 xu。 阻 抗 控 制 就 是 在 阻力 约束 下 的 机 械 手 末 
端 位 置 控制 。 

设 x 为 机 械 手 末端 位 置 向 量 ,关节 角度 gq 与 机 械 手 末端 位 置 向 量 x 关系 为 

x —h(q) (9. 22) 
且 
x —J (q)0q (9. 23) 
其 中 ,J(d ) 为 机 械 手 末端 的 Jacobian 信息 。 
机 械 手 末端 的 接触 阻力 为 下, F 与 位 置 误差 x. 一 x 有 关 , 动 力学 描述 为 
M, G, —) B, — --K, Gr, —3) —F, (9. 24) 
其 中 ,x. 为 接触 位 置 的 指令 轨迹 ,x (0)= 二 x.(0),M,,、B, 和 K,, 分 别 为 质量 .阻尼 和 刚度 系 
数 和 矩阵 。 
由 于 阻尼 控制 是 在 笛 卡 儿 坐 标 系 下 实现 ,为 了 实现 理想 接触 位 置 x. 的 轨迹 跟踪 ,为 此 
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图 9-16 带 有 阻力 约束 的 双关 节 机 械 手 


需要 通过 角度 动力 学 方程 求 得 笛 卡 儿 坐 标 系 下 动力 学 方程 ,针对 末端 位 置 双 关节 动力 学 模 
型 式 (9. 12) ,在 带 有 阻力 F. 的 笛 卡 儿 坐 标 系 下 转换 为 

D, (q)ž +C, (q0) +G. (q) +F, +4q:4:0) =F, (9. 25) 
其 中 ,D,(q)=J T ODI (0.C,(4.4)—J ^ (Q(C(q.40 DXGDJ ^ 40J (DJ QD. 
G, (=J  40GGD. llAG qq) |J GOL J CD SSICUSR C). 9)。 


9.4.2 阻抗 模型 的 建立 


在 阻抗 模型 中 ,阻抗 控制 目标 为 x 跟踪 理想 的 阻抗 轨迹 x, ,x。, 可 由 下 述 模型 求 得 
M,X,-B,x,-K,x,——F,- M,x, -B,x,. - K,x, (9. 26) 

其 中 ,xs(0)= 二 x.(0),xa(0) 二 x.(0)。 

根据 工作 空间 直角 坐标 与 关节 角 位 置 的 转换 及 工作 空间 关节 末端 节点 直角 坐标 (x e 
的 动力 学 模型 ,设计 加 在 关节 末端 节点 的 控制 律 F, ,并 通过 下, 与 + 之 间 的 映射 关系 , 求 出 
实际 的 关节 扭矩 r 。 

机 械 手 动态 方程 具有 下 面 特性 5 : 

CD 惯性 矩阵 D, (gq) 对 称 正 定 。 


(2) 矩阵 D.(q) 一 2C,.(qg,qg) 是 斜 对 称 的 。 
9.4.3 滑 模 控制 器 的 设计 


设 xs() 是 在 工作 空间 中 的 理想 轨迹 , 则 xs(z) 和 xu(t) 分 别 是 理想 的 速度 和 加 速度 。 
定义 
e) -—x40)—x(G) 
x,G) — x4G) - AeGQG) 
sG)— x,G) —xG) — eG) -- AeG) 
其 中 ,A 是 一 个 正定 矩阵 。 
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控制 器 设计 为 
F, —D,(q0X, - C, (q qox, +G, (q4) +F, +Ks + ntanh (=) (9. 27) 


H'BIK0.6720, 
将 控制 律 式 (9. 27) 代 入 式 (9. 25) ,得 


D.(qg)xz 二 C.(q qz 十 G. (4) +44:44) 


=D, (DX, +C, (0,4), +G: (q) - Ks + qtanh( >) 


将 x-—x,—s.x—x,—s 代 人 上 式 得 ， 
D, (që +C, («ds 十 Ks + qtanb(7-) — A(a ġġ) =0 
由 于 D,(dq) 为 对 称 正 定 , 则 可 定义 Lyapunov 函数 


V —5'D, Gs 


í NM. aM 
y —s'D,s-- 35 Ds 


: : : Y ous 
由 于 和 矩阵 D,(q) 一 2C,(q,qg) 是 斜 对 称 的 , 则 5 (D, —2C)s —0, Bl s  D,s s^ Cs fX 
入 上 式 得 
V —5 D, - s'C,s =s" (Di +C,s) =s" (— Ks — gtanh 7--- A4 4) ) 
根据 引 理 9. 1 和 引 理 9. 2, 有 
ji; (MN s è s T s T d» dn 
s ( 7tanh AG. 4) v tanh (2 )+s A(Cq.q.q) 


«— y | s ll 4- que 4- s ACq q 44) < me 
其 中 ,一 0. 2785. 
则 


V <—s"Ks + ne — À min (COS! s + me 
2À min (K) 1 


= “iD. 2 2 À max (D )s* s d- yep < — 24V +b 


À min CK) 


HR sA na (DF A mn KODA D, 和 K REKE A= 一 了， ,b= Ne. 


根据 V 志 一 24V 十 6 ,采用 引 理 9. 3, 可 得 


e aa 
2At 


VG) € e" "y, Ese | cr n a PE meu. Huy 


0 


—2AG—t4) —2AG—t4) 


= "vao za zz f 
则 


268 «(|| 机 器 人 控制 系统 的 设计 与 MATLAB 仿 真 ， 基本 设计 方法 (第 2 版 ) 


limV G) < o (9. 28) 


根据 式 (9. 28) ,跟踪 误差 和 误差 导数 渐进 收敛 ,收敛 精度 取决 于 e,X F m BI e 越 小 、K 
RKA 越 小 ,收敛 效果 越 好 。 


9.4.4 仿真 实例 


仿真 对 象 为 9. 2 节 的 对 象 ,考虑 平面 两 关节 机 械 手 ,机 器 人 的 动力 学 方程 为 
D(q0q -- C(q.q0q - G(q) —c 
其 中 ， 
T: á J-m;--2m;cosq; m; Fol 
m; --mscosq; m 
(p.a) e [Mesi — m, (q, NM 
m 4q,Ssinq» 0.0 
eii = ipei d- m;gcos(q, is 
m;gcos([q, +q2) 

ERF m, fiBixX M—P- pL 给 出 ,有 

M=[m; m, m, m, m;] 

P=[p! p: bs ps bsl 

Lep d dh h NI 
其 中 ,p, 为 负载 ,1， 8L, 分 别 为 关节 1 和 关节 2 的 长 度 ,P 是 机 器 人 自身 的 参数 向 量 。 机 
械 力 臂 实际 参数 为 p,—0.50,P—[1.66 0.42 0.63 3.75 1.25] .,—1,-—1. 

TE f& -F JL zs [8] rP B0) 8. RER PLE HC zu — 1. 0—0. 2cost «x; — 1. 04-0. 2sinz , 则 xa (00 = 
0.8,24(0) —0,24(0) —1.0,2,4 (0) —0. 2, BLA — NEBA 0.2. BD TECH x: — 
(1.0,1.0) 的 圆 。 初 始 条 件 为 x(0)= [0.85 1.05],x(0 —[0.0 0.0]. 

由 于 跟踪 轨迹 为 工作 空间 中 的 直角 坐标 ,而 不 是 关节 空间 中 的 角 位 置 ,应 按 9. 2 节 
式 (9.7) 和 式 (9. 8) 将 工作 空间 中 的 关节 末端 直角 坐标 (xi ,x,) 转 换 为 关节 角 位 置 (gq, 2D. 
采用 式 (9.9) 求 J(q) 和 J(gq), 采 用 式 (9.11) 可 将 ,转换 为 实际 控制 输入 c ,用 于 作 图 。 

仿真 中 ,首先 通过 式 (9. 24) 求 F, ,通过 式 (9. 25) 求 x, 然后 由 式 (9. 26) 求 xu。 接触 面 在 
zi 一 1.0 处 ,存在 以 下 两 种 情况 : 

OD 当 r, <1. 0 时 ,机 械 手 末 端 没有 接触 障碍 物 ,F, 二 [0 0]"。 

(2) 34 zx, «1. 0 时 ,机 械 手 末端 点 停留 在 触 障 碍 物 上 ,此 时 zi 三 1.0,zi 一 1.0,zi 一 
1.0; 

障碍 物 的 阻尼 参数 为 M, = 二 diag[1.0],B,= 二 diag[10] 和 K, —diag[ 50]. #18 A (q.q, 


i) sir ,请 模 控 制 器 取 式 (9.27) ,控制 器 的 增益 选 为 一 | 0 a -| ME 
0 150 0 30 


1.2,£—0.50, 仿真 结果 如 图 9-17 一 图 9-21 所 示 。 


position tracking of x1 axis 


position tracking of x2 axis 


Fel and Fe2 
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= m n deal xel l 
—7 practical xl 


= ideal xc2 
= = — practical x2 | 


time(s) 
图 9-17 末 关 节 节 点 的 位 置 跟踪 


external force of Fel 
= 77 — external force of Fe2 


5 10 15 20 
time(s) 


9-18 ” 末 关 节 节 点 的 外 力 
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tol of first link 
一 一 一 tol of second link| 


conrol input toll and tol2 


0 » 10 15 20 
time(s) 


图 9-19 ”关节 实际 控制 输入 r 


= ideal xc trajectory | 
1.15 


practical trajectory 


0.75 
0.75 0.8 0.85 0.9 0.95 


xl 


图 9-20 Xfx. 的 轨迹 跟踪 效果 
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0 5 10 15 20 
time(s) 


图 9-21 由 式 (9.26) 生 成 的 x, 轨迹 


9.4.5 仿真 中 的 代数 环 问题 


代数 环 是 控制 系统 MATLAB 仿真 中 不 可 避免 的 现象 ,之 所 以 称 为 代数 环 ,是 因为 输入 和 
输出 信号 是 同时 发 生 的 ,这 才 形 成 了 代数 环 。 一 般 情况 下 不 必 理 会 代数 环 ,让 Simulink 去 自行 
求解 代数 方程 即 可 ,但 有 时 代数 环 会 造成 Simulink 求解 出 现 困难 ,导致 仿真 中 止 。 如 果 能 让 这 
两 个 信号 不 同时 发 生 , 则 可 以 避免 代数 环 。 避免 代数 环 的 方法 有 许多 种 ， 一 种 有 效 的 方法 是 
在 输出 信号 后 面 接 一 个 延迟 环节 ,将 其 作为 输入 信号 馈 入 环 中 的 另 一 个 模块 ,如 果 这 个 延迟 的 
时 间 常 数 足够 小 , 则 有 望 用 这 种 方法 避 开 代数 环 ; 另 一 种 方法 是 在 输出 模块 后 接 一 个 低 通 滤 
波 器 ,避免 直 馈 现象 ,如 果 时 间 常 数 足够 小 , 则 可 以 很 好 地 避 开 代数 环 ""。 在 仿真 中 ,如 果 
按 正常 的 方式 仿真 ,可 以 发 现 S 函数 模块 子 程序 chap9_4Pe. m 陷入 了 代数 环 ,出 现 仿真 中 目的 
现象 。 解 决 的 办 法 是 在 出 现代 数 环 的 模块 后 面 引入 小 延迟 。" 或 低 通 滤波 器 于 =- 的 近 他 方 
法 ,可 以 很 好 地 吕 开 代数 环 的 问题 。 在 本 仿真 中 ,为 了 克服 代数 环 ,在 S 函数 模块 子 程序 chap9 
_4Fe. m 后 面 引入 低 通 滤波 器 元 = , 取 T —0.015, Ul, Simulink 主 程序 chap9_4sim. mdl。 


仿真 程序 如 下 : 
(1) Simulink 主 程序 : chap9_4sim. mdl。 
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(2) 输入 指令 S 函数 : chap9_4input. m, 


function [sys,x0,str,ts] = spacemodel(t, x, u, f lag) 
switch flag, 
case 0, i 
[ sys, x0, str, ts] = mdlInitializeSizes; 
case 1, 
sys = mdlDerivatives(t, x,u); 
case 3, 
sys = ndlOutputs(t,x,u); 
case (2,4,9) 
sys - []; 
otherwise 
error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0, str, ts] = mdlInitializeSizes 


Sizes - simsizes; 


sizes.NumContStates 


sizes.NumDiscStates = 


0 
0 
sizes. NumOutputs = 6; 
sizes.NumInputs = 0 
sizes.DirFeedthrough - 1 
sizes.NumSampleTimes = 1 


SyS = simsizes(sizes); 


x0 = [E 
str = []; 
ts - [00]; 


function sys = mdlOutputs(t, x, u) 
xcl-1-0.2*cos(t); 
dxcl-70.2* sin(t); 
ddxc1 = 0.2 * cos(t); 
xc2-1*40.2* sin(t); 
dxc2 = 0.2 * cos(t) ; 
ddxc2 = — 0.2 * sin(t); 
sys(1) = xc1; 

sys(2) = dxcl; 

sys(3) = ddxcl; 

sys(4) = xc2; 

sys(5) = dxc2; 

sys(6) = ddxc2; 


(3) 描述 阻力 正 , 的 S 函数 : chap9 4Fe. m, 


function [sys,x0,str,ts]- s function(t, x,u, flag) 
switch flag, 
case 0, 
[sys, x0, str, ts] = ndlInitializeSizes; 
case 3, 
sys = mdlOutputs(t,x,u); 
case (1,2, 4, 9} 
sys = []; 
otherwise 
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error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0, str, ts] = mdlInitializeSizes 
Sizes = simsizes; 


sizes.NumDiscStates  - 0; 
sizes.NumOutputs -2; 
sizes.NumInputs - 14; 
sizes.DirFeedthrough = 1; 
Sizes.NumSampleTimes = 0; 
Sys = simsizes(sizes); 
x0-[]; 

str-[]; 

ts-[]; 


function sys = mdlOutputs(t, x, u) 
xc 7 [u(1) u(4)]'; 

dxc = [u(2) u(5)]'; 

ddxc = [u(3) u(6)]'; 


Mm - [1 0;0 1]; 
Bm - [10 0;0 10]; 
Km - [40 0;0 40]; 


x1 = u(7);dx1 = u(8);ddxl = u(9); 
x2 = u( 10) ;dx2 = u(11);ddx2 = u(12); 


xp= [x1 x2]'; 
dxp = [dx1 dx2]'; 
ddxp = [ddx1 ddx2]'; 
if x1»- 1.0 
xp 7 [1.0 xp(2) ]';dxp- [0 dxp(2) ]';ddxp- [0 ddxp(2)]'; 
end 


Fe = Mm * (ddxc - ddxp) + Bm * (dxc — dxp) + Km * (xc- xp);  *(9.24) 
if x1«1.0 

Fe- [0 0]'; 
end 


sys(1) = Fe(1); 
sys(2) = Fe(2); 


(4) 阻抗 轨迹 x, 生成 S 函数 : chap9 4xd. m, 


function [sys,x0, str,ts] = s_function(t, x,u, flag) 
switch flag, 
case 0, 
[sys, x0, str, ts] = ndlInitializeSizes; 
case 1, 
sys = ndlDerivatives(t,x,u); 
case 3, 
sys = ndlOutputs(t, x,u); 
case (2, 4, 9 ) 
sys = []; 
otherwise 
error(['Unhandled flag = ',num2str(flag)]); 
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end 
function [sys, x0, str,ts] = mdlInitializeSizes 
sizes = simsizes; 

sizes.NumContStates  - 


M 


sizes.NumDiscStates  - 


€ 


sizes.NumOutputs - 
sizes.NumInputs = 
sizes.DirFeedthrough = 
sizes.NumSampleTimes = 


S 


EM 


oOrnP 000 à 
M. 


Sys = simsizes(sizes); 

x07 [0.80 1.0 0.2* pi]; 5 xd(0) = xc(0), dxd(0) = dxc(0) 
str-[]; 

ts- []; 

function sys = mdlDerivatives(t, x, u) 

xc = [u(1) u(4)]'; 

dxc = [u(2) u(5)]'; 

ddxc = [u(3) u(6)]'; 


Mm= [1 0;0 1]; 
Bm= [10 0;0 10]; 
Km = [50 0;0 50]; 


Fe-[u(7) u(8)]'; $5&(9.24) 


xd - [x(1);x(3)]; 
dxd - [x(2);x(4)]; 
ddxd = inv(Mm) * (( - Fe + Mm * ddxc + Bm * dxc + Km * xc) - Bm * dxd- Km * xd); %(9.26) 


sys(1) = x(2); 
sys(2) = ddxd(1); 
sys(3) = x(4); 
sys(4) = ddxd(2) ; 


function sys = mdlOutputs(t, x,u) 
xc 7 [u(1) u(4)]'; 

dxc = [u(2) u(5)]'; 

ddxc = [u(3) u(6)]'; 


Mm= [1 0;0 1]; 
Bm = [10 0;0 10]; 
Km= [40 0;0 40]; 


Fe-[u(7) u(8)]'; %(9.24) 


xd 7 [x(1);x(3)]; 
dxd - [x(2);x(4)]; 
S= inv(Mm) * (( - Fe + Mm * ddxc + Bm * dxc + Km * xc) - Bm * dxd - Km * xd) ; % ddxd 


sys(1) = x(1); % xd 
sys(2) = x(2); 
sys(3) - S(1); 
sys(4) = x(3); 
sys(5) = x(4); 
sys(6) - S(2); 
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(5) Tui as JA SEX SEP: chap9 4system. m, 


function [sys, x0,str,ts] = s function(t, x, u, flag) 
switch flag, 
case 0, 
[sys, x0, str, ts] = ndlInitializeSizes; 
case 1, 
sys = ndlDerivatives(t, x,u); 
case 3, 
sys = ndlOutputs(t, x, u); 
case (2, 4,9] 


sys = [E 
otherwise 

error(['Unhandled flag = ',num2str(flag)]); 
end 


function [sys, x0, str, ts] = mdlInitializeSizes 


sizes = simsizes; 


sizes.NumContStates 


sizes.NumDiscStates 
sizes.NumOutputs 


LU 
or 0 00. 5 


sizes.NumInputs 


sizes.DirFeedthrough 


sizes. NumSampleTimes 

Sys = simsizes(sizes); 
x0-[0.80 1.00]; 5$ x(0) = xc(0) 
str-[]; 

ts-[]; 

function sys = mdlDerivatives(t,x,u) 
xdl = u(1);dxdl = u(2);ddxdl = u(3); 

xd2 = u( 4) ; dxd2 = u(5);ddxd2 = u(6); 


Fel = u(7);Fe2= u(8); 
Fe = [Fel Fe2]'; 


11 =1;12=1; 
P= [1.66 0.42 0.63 3.75 1.25]; 
g=9.8; 


L-[11^2 12^2 11 * 12 11 12]; 
pl=0.5; 


M=P+plxL; 

Q= (x(1)^2* x(3)^2- 11^2 - 12^2)/(2 » 11 * 12); 
q2 = acos(Q) ; 

dq2 = - 1/sqrt(1- Q^2); 


A- x(3)/x(1); 
pl = atan(A); 


d pl-1/(1* A2); 


B= sgrt(x(1)^2* x(3)^2* 11^2- 12?2)/(2* 11 * sart(x(1)^2* x(3)^2)); 


276 «Q]| 机 器 人 控制 系统 的 设计 与 MATLAB 仿 真 : 基本 设计 方法 (第 2 版 ) 


p2 = acos(B) ; 
d p2= - 1/sqrt(1- E^2); 


if q2»0 
ql = p1 - p2; 
dql = d_pl - d_p2; 
else 
ql = pl + p2; 
dql =d pl+d p2; 
end 
J= [ - sin(gl)— sin(gl +q2) - sin(gl + q2); 
cos(q1) + cos(q1 * q2) cos(al + a2)]; 
dJ-7[-dal*cos(q1)- (dal + dg2) * cos(q1 * q2) - (dal + dq2) * cos(al + q2); 
— dg1 * sin(q1) - (dgql + dq2) * sin(ql + q2) - (dq1 + dq2) * sin(ql + a2)]; 


D= [M(1) * (2) * 2* M(3) * cos(q2) M(2) * M(3) * cos(q2); 
M(2) + M(3) * cos(q2) M(2)]; 
C-[-M(3) *dq2* sin(q2) —-M(3) * (dal + dq2) * sin(q2); 
M(3) * dql * sin(gq2) 0]; 
G= [M(4) * g* cos(q1) +M(5) * gx cos(ql + q2); 
M(5) * gx* cos(ql + q2)]; 


Dx= (inv(J))'*D* inv(J); 
Cx= (inv(J))'* (C- Dx inv(J) * d J) * inv(J); 
Gx = (inv(J))'* G; 


el = xd1 - x(1); 
e2 = xd2 - x( 3); 
del = dxd1 - x(2); 
de2 = dxd2 - x(4); 
e= [el;e2]; 

de = [del1;de2]; 


Hur = 30 * eye(2) ; 


r= de + Hur # e; 


dxd = [ dxd1 ; dxd2 ]; 
dxr = dxd + Hur * e; 
ddxd = [ ddxd1 ; ddxd2 ] ; 
ddxr = ddxd + Hur * de; 


K= 150 * eye(2); 

epc = 0.50; 

xite= 1.2; 

Fx = Dx * ddxr + Cx * dxr + Gx + K * r + Fe + xite * tanh(r/epc); 


Delta = [sin(t) sin(t)]'; 
dx= [x(2);x(4)]; 


S= inv(Dx) * (Fx- Cx * dx- Gx— Delta- Fe); 
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sys(1) = x(2); 

sys(2) - S(1); 

sys(3) = x(4); 

sys(4) = S(2); 

function sys = mdlOutputs(t, x, u) 
xdl = u(1);dxdl = u(2);ddxdl = u(3); 
xd2 = u(4);dxd2 = u(5);ddxd2 = u(6); 


Fel = u(7);Fe2 = u(8); 
Fe-[FelFe2]'; 


11-21;12-21; 
P= [1.66 0.42 0.63 3.75 1.25]; 
g79.8; 


G= [1152 12^2 11*32 11 12]; 
pl-20.5; 


M=P+pl* L; 
Q7 (x(1)^2* x(3)^2- 11^2- 12^2)/(2 » 11 x 12); 
q2 = acos(0) ; 

dq2 = -1/sqrt(1- Q^2); 


A7 x(3)/x(1); 
pl = atan(A); 
d pl 2 1/(1* £2); 


B= sqrt(x(1)^2 + x(3)^2* 1172- 122)/(2*11* sart(x(1)^2* x(3)^2)); 
p2 = acos(B); 
d p27 - 1/sqrt(1- B^2); 


if q2»0 
ql = pl - p2; 
dql =d_p1 -d_p2; 
else 
ql = pl + p2; 
dql = d_p1 + d_p2; 
end 
J= [ - sin(ql) - sin(q1 + q2) - sin(q1 + q2); 
cos(ql) +cos(ql + q2) cos(al + q2)]; 
d_J = [ - dq1 * cos(q1) - (dq1 + dq2) * cos(q1 + q2) - (dal + dq2) * cos(q1 + q2); 
— dgl * sin(ql) - (dql + dq2) * sin(gql + q2) - (dq1 + dq2) * sin(q1 + q2) ]; 


D=[M(1)+M(2)+2*M(3) * cos(q2) M(2) + M(3) * cos(q2); 
M(2) + M3) * cos(q2) M(2)]; 
C-[-M(3) *dq2* sin(q2) - M(3) * (dgl + dq2) * sin(q2); 
M(3) * dql * sin(q2) 0]; 
G- [M(4) * gx cos(q1) * M5) * gx* cos(qgl + q2); 
M(5) * g* cos(q1 + q2)]; 


Dx= (inv(J))'* D * inv(J); 
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Cx= (inv(J))'* (C- D* inv(J) * d J) * inv(J); 
Gx= (inv(J))'*G; 


el = xdl- x(1); 
e2 = xd2 = x(3); 
del = dxdl - x(2); 
de2 = dxd2 - x(4); 
e= [el;e2]; 

de = [de1;de2]; 


Hur = 30 * eye(2) ; 
r= de + Hur * e; 


dxd = [dxdl;dxd2 ] ; 
dxr = dxd * Hur * e; 
ddxd = [ddxdl ; ddxd2] ; 
ddxr = ddxd + Hur * de; 


K= 150 * eye(2) ; 

epc = 0.50; 

xite= 1.2; 

Fx = Dx * ddxr + Cx * dxr + Gx + K * r + Fe + xite * tanh(r/epc); 


Delta = 1.0 * sin(t); 
dx= [x(2) ;x(4)]; 


S= inv(Dx) * (Fx- Cx * dx - Gx- Delta - Fe); 
tol = J' * Fx; 


sys(1) = x(1); 
sys(2) * x(2); 
sys(3) - S(1); 
sys(4) = x(3); 
sys(5) = x(4); 
sys(6) = 5(2); 
sys(7:8) = tol(1:2); 


(6) 作 图 程序 ; chap9. 4plot. m, 


close all; 


figure(1); 

subplot(211); 

plot(t,xe(t,1), £r—— '".t;x(:,1), "b", "Linewidth',2); 
xlabel('time(s)');ylabel('position tracking of xl axis'); 
legend('ideal xcl', practical x1'); 

subplot(212); 

plot(t,xc(:,4), 'r',t,x(:,4), 'b-- ', 'linewidth',2); 
xlabel('time(s)');ylabel('position tracking of x2 axis'); 
legend('ideal xc2', 'practical x2'); 


figure(2); 
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plot(t,xd( :,1), tS tm,2), 'b.— *, 'Tinewidkli^, 2); 
xlabel('time(s)');ylabel('Fel and Fe2'); 
legend( 'external force of Fel', external force of Fe2'); 


figure(3); 

plot(t,x(:,;3),'r',t,x( :,4);; 'b- ', "TinewidEh';2); 
xlabel('time(s)');ylabel('conrol input toll and tol2'); 
legend('tol of first link', tol of second link'); 


figure(4); 

plot(xc(:,1),xc(:,4), 'r', 'linewidth',2); 

hold on; 

plot(x(:,1),x(:,4), 'b-- ', 'linewidth',1); 
xlabel('x1');ylabel('x2'); 

legend('ideal xc trajectory', 'practical trajectory'); 


figure(5); 

plot(t,xd(:,1), 'r',t,xd(:,4), 'b-- ', 'linewidth',2); 
xlabel('time(s)');ylabel('xd'); 

legend( 'xdl', 'xd2'); 


9.5 受 约束 条 件 下 双关 节 机 械 手 末端 力 及 关节 角度 的 
滑 模 控制 
受 约束 机 器 人 的 运动 控制 包含 两 方面 的 内 容 : 机 械 手 的 位 置 控制 和 末端 与 约束 面 之 间 
的 接触 力 控制 。 一 般 而 言 , 位 置 控制 较 容易 实现 ,例如 采用 传统 的 独立 关节 PD 控制 等 , 接 


触 力 控制 则 相对 较 难 。 机 械 手 力 /位 置 混合 控制 相关 研究 成 果 较 多 ,代表 性 的 成 果 见 文献 
[10,11,12]. 


9.5.1 问题 的 提出 
带 有 垂直 约束 的 双关 节 机 械 手 示意 图 如 图 9-22 Br, 


图 9-22 带 有 垂直 约束 的 双关 节 机 械 手 
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设 x 为 机 械 手 末端 位 置 向 量 2 98 7; EO. pax) —0. Rx —h q), WARDER OS 
Deg) —4(h(q) —0 (9. 29) 
约束 方程 的 Jacobian 信息 为 


J;(q)= 


双关 节 机 械 手 方程 为 


ID) A$(x) ox A$(x) 2h (q) 
dq = ax dq ax dq 


(9. 30) 


D(q0)d - C(q.d)d +G) - v, =r (9. 31) 
其 中 ,gq 二 [qt g] ,D (gq) 为 2X2 MEERE, Cg) H 2X 2 阶 离心 和 哥 氏 力 项 ， 
G(q) 为 2X1 阶 重 力 项 ,rr 为 约束 力 。 


9.5.2 模型 的 降 阶 
由 于 机 械 手 动力 学 模型 是 约束 的 ,因此 需要 利用 约束 条 件 对 模型 进行 降 阶 。 约 束 力 表 
示 为 
t; —J 49A (9. 32) 
由 式 (9. 29) 求 导 ,可 得 
J,GDq —0 (9. 33) 


由 图 9-22 可 见 , 由 于 双关 节 机 械 手 受到 一 个 力 的 约束 , 故 双关 节 机 械 手 自由 度 由 2 个 变 为 1 
个 ,不 妨 取 g 为 描述 约束 运动 的 变量 ,gq, 为 剩余 的 元 余 变量 , 则 gq, 可 由 gq, 表示 为 


qs =YV(gi) (9.34) 
则 
.fg uis : 
q—]|. |= |2v(q). | ^L(G)q 
2 z, N 
oqi 
4 —L(G)Odq HLD, 
1 
其 中 ,L(g1)= IPaq). 
dq, 
将 上 式 代 入 式 (9. 31) 可 得 
D(q) lq gi HLDG -CG;q)LG G +G) Hr =r 
Bp 


D(L (q ë, +DL) HCL) g HGU) -J1 (40A =r 
则 带 有 约束 的 双关 节 机 械 手 方程 又 可 写 为 
D, (qiddi +C: lq) +G lq) +J; Cgi) =r (9. 35) 
其 中 ,Di(g1)==D(q)L(gi),Ci(gi ,91)=D (gq)L (q, ) --C(q.q DL (qı ); G; (q12—G(q), 
JG =J; (q). 
HFJ, DL DSL"); Cd 一 0, 说 明 式 (9.35) 生 成 的 两 个 方程 为 线性 相关 , 因 
此 ,在 仿真 中 通过 式 (9. 35) 求 4 ,可 得 
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J; (9 一 rr 一 Do 一 Ci)d — Gi (qı) 

在 仿真 中 ,采用 如 下 三 种 情况 求 力 4 fü: 

(1) 当 J; 二 0 时 , 则 可 根据 J; (qi) 的 表达 取 4 值 。 

(2) 当 J,(1) 冯 0 时 , 则 可 取 

1 
= 
(3) 24 J, (20 0 时 , 则 可 取 

1 
"Jud 
由 于 式 (9. 38) 生 成 的 两 个 方程 为 线性 相关 ,相关 系数 为 L(g,), 则 由 式 (9. 36) 和 式 (9. 37) 求 得 
BS A 相等 。 

需要 说 明 的 是 ,在 实际 工程 中 , 力 * 的 值 通过 力 传感器 测 得 。 

式 (9. 35) 的 左右 两 边 分 别 乘 以 L'(g,), 可 得 
L'(g)Di(g)g t+L (gCi(gi sgidia - L' GGG) HLC J] (400A =L" (gi) € 
即 


À (t (1D) —D,CDq; —C, Dg, —6G,CD) (9. 36) 


A (r (2) =D; (2g; —C,0D4, —G,CD) (9. 37) 


D, (Qd, - C, G1 d)di +G lq) =L"r (9. 38) 

式 (9. 38) 即 为 降 阶 后 的 双关 节 机 械 手 模型 。 式 (9. 38) 满 足 如 下 几 个 性 质 5 ; 

(1) 定义 Di (q)) 2L" (40D, (1) 5L q DL (q) 4D, (12750, 

(2) S£ X C, (1 44) L' GC, (Qd) 9L! G0) OD GOL (10 7C GP 4 2L Gp) , Bl 
D, (q) —2C, (qi,41) 具 有 和 斜 对 称 特 性 。 

(3) J,(q L(g =L" (qi); (0,0) —0, 

Wt gu(z) 为 理想 的 角度 指令 ,ry 为 理想 的 受 约束 力 , 且 满足 更 (gu) 王 0,ry 一 JJ (4024. 
控制 目标 为 q(t) 跟 踪 qa FI c, 跟踪 7 。 


9.5.3 控制 律 的 设计 
由 于 gs (2) 为 gqg1 GO BS PRURCL q CORRER qu GO BI qi GRRE qu (D. 4E X 


el 一 dl 一 qi 
du 一 qu 十 Ae， 
ri 一 du 一 0 一 2 十 Ae， 
e, 一 人 d 一 从 
rn —L(qi)ri 
其 中 ,4 人 之 0。 
控制 律 设 计 为 
t=Di(gi1)gn t Ci(qisdda c Gi Gu) Kari Ti (002, (9. 39) 
其 中 ,K,>0， | 
用 于 控制 力 的 项 为 
À, =A T K;e,; 


X'B.K,0, 
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于 是 
À, —À =e; 4 K;e, 三 (1 十 有 ;Yes (9. 40) 

将 控制 律 式 (9. 390 AX C9. 35) 中 ,得 

D, (q)0di +C: lgi ogi) +G: lq) +J] (4104 

—Di;(qi44 t Ci(idi da Gilg) + Rr 十 JiD)A， 

则 

D, (G7, +C: (GidDri Kora —J; (gqg1) A 7,) (9. 41) 
左右 两 边 都 乘 以 L (gq), 可 得 

L' (qj) OD, G7; C; (gi Gr t+ Kor? =L" (90), (1) —A) 


根据 性 质 (1) 一 性 质 (3) ,可 得 
D, (q)07, 十 C Gi 5d ri - L'(GjK,r;, 一 0 (9. 42) 
9.5.4 稳定 性 分 析 
Lyapunov PK Zi f Jj 
V-4D, Gori (9. 43) 
DU 


. l 
V —r (D. G^ + D. Gri) 


考虑 到 D, (G0 —2C, Gi g ) 的 斜 对 称 特性 ,并 将 式 (9.42) 代 入 上 式 , 有 


V 2«riCGDi (q)07, 3- C, (41,54 r) ri C—E' (gi ) 开 ril ) 一 —riK,ri «o 


由 于 V 是 半 负 定 的 , 且 K, 为 正定 , 则 当 V 寺 0 BE VÉ ra 0, 7,,250; Bl r, 20,727,250, e 三 
ei70, Hi LaSalle 定理 可 知 ,t 一 ceo 时,ei 一 0,ci 一 0。 

由 于 ra 70,7, 70,7,70, RHEO. 41) RTI ,A —2,950 BEI HE SX C9. 400 RT AIL e; —0, 
由 LaSalle 定理 知 ,:— oo BT ,A—A, , Hl Sc REX T (OK). 


9.5.5 仿真 实例 


选 二 关节 机 器 人 系统 (不 考虑 摩擦 力 和 干扰 ) ,其 动力 学 模型 为 
D(q)d -- C(q.d)d - G(q) t c, —c 


其 中 ， 
bı +p: + Epacosq; p; pcosq: 
Dq) = 
P2 + pcosq: pe 
2 — pid;sinqs  — ps(q, -q;)sinq; 
C(q T = . ] 
PpP3qd1sing» 0 
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b.gcosq, + psgcos(q, d- q5) 
G(q)— 
b«gcos(q, o q;) 
取 也 一 [2. 90 0.76 0.87 3.04 0.87]', 
如 图 9-22 所 示 ,末端 在 工作 空间 中 的 位 置 为 


xı =0 
£ =li 4 li — 2l,lzcos(2q1) 
如 图 9-22 所 示 ,约束 函数 为 ri =ġ(x)=0, BI 
$ (q1) 一 Licosql 十 lzcos(dql 十 qz) 一 0 
HT £9 38 Jr E (0 — 4 (h C40) — 8 Cq) = l,cosqi +l:cos (qi +q), DUI A HE 
xx (9. 30) ,可 得 


J; = 


9 
PLU. - [- h sing, 一 asin(g + qs) — hsin(qi + qz)] 


X qı +q: =x HE .q;—0.q, =x, M] J, (q) — 0, P A 9-22 nf UL , JE EE PT T LRL S I6] E TC 
置 ,显然 ,此 时 4 二 0。 

M gi 十 gs 关 x 时 ,J (1) 关 0,J (2) 冯 0, 则 可 按 式 (9. 36) 或 式 (9. 37) 求 4。 

WC 1,1, =1.0, MH $(q1)==Lcosgqi 十 lscos(qi 十 qs) 二 0 可 得 cosg, +coslqi +q:)=0,4¢R 
据 图 9-22 可 知 


di 十 gz <T, 0<q S 


co|a 


OSg anr 


则 
cos(q, 十 qz) 一 cos(Cr 一 01) 
根据 上 式 可 得 gj 十 gq, 二 x 一 gi，, 即 
qs —2—2q, 
HF q;— VG D. VG) — x— 2q ,从 而 
; 1 
L(gi)= |9W(q) -| | 
ðq, T 
可 对 性 质 (3) 进 行 验 证 : 
J;(q)L(g1)=[— l;sing, — lF;sin(q, 4- gq2) -tsina +41 | 
— — [,sinq, — lzsin(q;, +q) + Zl;sin(q, 十 ay) 
— — [,sinq, 十 /2sin(ql 十 qz) 
— — [,sinq; 十 /2sin(r 一 qi) 一 0 
被 控 对 象 为 式 (9. 38) ,被 控 对 象 q qu 初始 状态 为 [1.4 0]。 位 置 指令 为 qu 一 0.8 十 
0. 5cost ,理想 的 约束 力 为 44=10sint。 采 用 控制 器 式 (9. 39) ,控制 器 参数 为 天 ,一 


10 0 
| : si .K,—0.8,A—5.0, 仿真 结果 如 图 9-23 一 图 9-25 所 示 。 
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1.5 
A ideal qid 
wm 1 — — —- practical ql 
£ 
[5] 
E 
v 0.5 
I: 
E 

0 O 
0 5 10 15 20 25 30 
time(s) 


angle speed tracking of qi 


time(s) 


图 9-23 关节 1 的 角度 及 角速度 跟踪 


conrol input toll and tol2 


0 5 10 15 20 25 30 
time(s) 


图 9-24 关节 1 和 关节 2 的 控制 输入 


入 tracking 


time(s) 


9-25 ”约束 力 的 跟踪 及 跟踪 误差 
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), tracking error 
5 o» p 
N © N L CN 


"0 5 10 15 20 25 30 
time(s) 
9-25 (£D 


仿真 程序 如 下 : 
(1) Simulink 主 程序 : chap9_5sim. mdl。 


Chap9 5input 


chap9 5ctri 


S-Functionà 


2 
chap9 Splant 


S-Functiont 


(2) 位 置 指令 子 程 


hr 


: chap9. 5Sinput. m, 


function [sys, x0, str,ts] = spacemodel(t,x, u, flag) 
switch flag, 
case 0, 
[sys, x0, str, ts] = ndlInitializeSizes; 
case 1, 
sys = ndlDerivatives(t,x,u); 
case 3, 
sys = ndlOutputs(t,x,u); 
case (2,4,9) 
sys- []; 
otherwise 
error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0,str,ts] = mdlInitializeSizes 
Sizes - simsizes; 


sizes.NumContStates 


sizes.NumDiscStates 
sizes. NumOutputs 


sizes.NumInputs 


sizes.DirFeedthrough 


nl 
k^» Fo oo 
~ 


sizes. NumSampleTimes 
sys = simsizes(sizes); 


x0 = []; ， 
str = []1; 
ts = [00]; 


function sys = mdlOutputs(t, x, u) 
qld= 0.8+0.5* cos(t); 
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sys(1) = qid; 
(3) 控制 器 子 程序 : chap9 5ctrl. m, 


function [sys,x0, str,ts] = s function(t, x,u, f lag) 
switch flag, 
case 0, 

[sys, x0, str, ts] = ndlInitializeSizes; 
case 3, 

sys = ndlOutputs(t,x,u); 
case (1,2, 4, 9 } 

sys = []; 
otherwise 

error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0, str, ts] = ndlInitializeSizes 


sizes - simsizes; 


sizes.NumContStates 


sizes.NumDiscStates 
sizes.NumOutputs 


M 
or 0000 
TS 


sizes.NumInputs 


sizes.DirFeedthrough 


sizes.NumSampleTimes 

sys = simsizes(sizes); 
x0-[]; 

str-[]; 

ts=[]; 

function sys = mdlOutputs(t, x, u) 
qdl = u(1); 

ql = u(2); 

dql = u(3); 

q2 - u(4); 

dq2 = u(5); 


dqdl = -0.5* sin(t); 
ddqdl = - 0.5 * cos(t); 


1121;1251; 
p=[2.9 0.76 0.87 3.04 0.87]; 
g79.8; 


D- [p(1) * p(2) * 2* p(3) * cos(q2) p(2) + p(3) * cos(q2); 
p(2) * p(3) * cos(q2) p(2)]; 
C-[-p(3) * dq2 * sin(q2) - p(3) * (dql + dq2) * sin(q2); 
p(3) * dal * sin(q2) 0]; 
G= [p(4) * g* cos(q1) + p(5) * gx cos(q1 + q2); 
p(5) * g * cos(ql + q2)]; 


J= [ -11 * sin(q1)- 12 * sin(ql +q2) - 12* sin(ql + q2)]; 


L-[1 -2]'; 
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D1-D*L; 
C1-C*L; 
G1-G; 


el = qdl - ql; 
del = dqd1 - dq1; 


Fai= 5.0; 

dqrl = dqd1 + Fai * el; 
ddqrl = ddqd1 + Fai * del; 
rl = del + Fai * el; 


lambda = u(6); 
lambda d= 10 * sin(t); 


e lambda = lambda d - u(6); 
rll1-2L*rl; 


K lambda - 10; 
lambda r= lambda d + K lambda * e lambda; 
555555555; 535 5; 55555; 5556955955595 953595 85 095 


Kp= [5 0;0 5]; 
tol = D1 * ddqr1 + C1 * dqrl + G1 + Kp* r L1 + J' * lambda r; 


tolf = J' * lambda; 


sys(1) = tol(1); 
sys(2) = tol1(2); 
sys(3) = lambda; 


(4) 被 控 对 象 子 程序 : chap9 5plant. m, 


function [sys,x0, str,ts] = s function(t, x, u, flag) 
switch flag, 
case 0, 

[sys, x0, str, ts] = mdlInitializeSizes; 
case 1, 

sys = ndlDerivatives(t,x,u); 
case 3, 

sys = mdlOutputs(t,x,u); 
case (2, 4, 9 ) 

sys = []; 
otherwise 

error(['Unhandled flag = ',num2str(flag)]); 
end ; 
function [sys, x0, str,ts] » mdlInitializeSizes 
Sizes - simsizes; 
sizes.NumContStates 


LU 
Oo N 
T 


sizes. NumDiscStates ^ 


sizes.NumOutputs 55 
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sizes. NumInputs -3; 
sizes.DirFeedthrough = 1; 
sizes.NumSampleTimes - 0; 

sys = simsizes(sizes); 

x07 [1.4 0]; ' 
stre-[] 


ts-[]; 
function sys = mdlDerivatives(t, x,u) 
tol-u(1:2); 


q1-x(1);q27pi-2*x(1); 
dql = x(2);dq2 = - 2 * dgl; 


p= [2.9 0.76 0.87 3.04 0.87]; 
g-79.8; 


D- [p(1) * p(2) * 2* p(3) * cos(q2) p(2) + p(3) * cos(q2); 
p(2) * p(3) * cos(q2) p(2)]; 
C-[-p(3) * dq2 * sin(q2) - p(3) * (dal + dq2) * sin(q2); 
p(3) * dq1 * sin(q2) 0]; 
G-[p(4) *g*cos(a1) * p(5) * gx cos(ql + q2); 
p(5) * g* cos(q1 + q2)]; 


L-[1 -2]*; 


DL=L'*D* L; 
CL2L'*C*L; 
GL-L'*G; 


ddql = (L' * tol- CL * dq1 - GL) /DL; 


sys(1) = dal; 
sys(2) = ddql ; 
function sys = mdlOutputs(t, x, u) 
1121;12-21; 
tol-u(1:2); 


ql=x(1);q2= pi-2* x(1); 
dgl = x(2);dq2 = -2 * dal; 


p=[2.9 0.76 0.87 3.04 0.87]; 
g79.8; 


D= [p(1) * p(2) * 2 * p(3) * cos(q2) p(2) + p(3) * cos(q2); 
p(2) + p(3) * cos(q2) p(2)]; 
C-[-p(3) * dq2 * sin(q2) - p(3) * (dal + dq2) * sin(a2); 
p(3) * dql * sin(q2) 0]; 
G= [p(4) * g* cos(q1) * p(5) * g* cos(al * q2); 
p(5) *g* cos(ql + q2)]; 
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"Ft 


L-[1 -2]*; 


DL-L'*D*L; 
CL-L'*C*L; 
GL-L'*G; 


ddql = DIA (L' * tol - CL * dql - GL); 


D)-2D*L; 
C1-C*L; 
G1 =G; 


J= [ -11 * sin(q1) - 12 * sin(ql + q2) - 12 * sin(q1 + q2)]; 
temp = tol- D1 * ddq1 - C1 * dq1 - G1; 


if ql + q2 == pi 
lambda = 0; 


else 


lambdal = temp(1)/J(1);lambda2 = temp(2)/J(2); % lambdal = lambda2 


lambda = lambdal; 
end 
sys(1) = x(1); 
sys(2) = x(2); 


sys(3)  pi- 2* x(1); % q2 
sys(4) = -2*x(2); % dq2 


sys(5) = lambda; 
(5) 绘图 子 程序 : chap9_5plot. m. 


close all; 


figure(1); 

subplot(211); 

plot(t,qid(:,1), 'r',t,x(:,1), 'b-- ', 'linewidth',2); 
xlabel('time(s)');ylabel('angle tracking of q1'); 
legend('ideal qld', 'practical q1'); 

subplot(212); 

dqdl= -0.5* sin(t); 

plot(t,dqdl, 'r',t,x(:,2), 'b-- ', 'linewidth',2); 
xlabel('time(s)'); ylabel( angle speed tracking of q1'); 
legend('ideal dq 1 d','practical q 2'); 


figure(2); 

piot(t,tol(:,l),'r',t,tol(:,2), b ', 'Tinewidth',2); 
xlabel('time(s)');ylabel('conrol input toll and to12'); 
legend('tol of first link','tol of second link'); 


figure(3); 
subplot(211); 
plot(t,10 * sin(t), 'r-- ',t,x(:,5), 'b', 'linewidth',2); 
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[2] 
[3] 
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[6] 
[7] 
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[10] 


[11] 


[12] 


13] 
14] 


rA ri 


xlabel('time(s)');ylabel( MÀ tracking'); 
legend( MAd', à"); 

subplot(212); 

plot(t,10 * sin(t) ~ x(:,5), 'r', 'linewidth',2); 
xlabel('time(s)'); ylabel('VXÀ tracking error'); ' 
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第 10 章 


CHAPTER 10 


重复 控制 的 基本 原理 及 
设计 方法 


10.1 重复 控制 的 基本 原理 


20 世纪 80 年 代 开 始 ,重复 控 制 方法 在 日 本 兴起 。 重 复 控制 (Repetitive Control) 最 先 
由 日 本 学 者 内 山 ( 日 本 东北 大 学 ) 在 一 篇 有 关机 器 人 控制 的 文章 中 提出 号 。 

重复 控制 方法 的 目标 是 设计 一 个 针对 周期 信号 的 跟踪 控制 器 或 者 扰动 补偿 器 ,只 需 基 
于 过 去 周期 的 误差 信号 ,除了 使 用 当前 控制 误差 外 ,还 “重复 ”使 用 了 上 一 周期 的 误差 ,并 与 
当前 控制 误差 倒 加 在 一 起 ,作为 偏差 控制 信和 号。 重复 控制 方法 能 够 大 大 提高 系统 跟踪 周期 
信号 的 能 力 ,抑制 周期 性 的 干扰 ,具有 较 好 的 跟踪 鲁 棒 性 能 。 

重复 控制 最 直接 的 解决 方案 是 应 用 内 模 原 理 构 造 内 模 控 制 器 。 在 控制 器 中 包含 周期 信 
号 的 模型 ,以 获得 无 差 的 渐进 跟踪 特性 。 如 果 输 入 信号 模型 具有 无 穷 的 谐 波 成 分 (例如 方 波 
信号 ), 则 控制 器 必须 包含 无 穷 维 的 信号 模型 。 在 知道 信号 周期 的 情况 下 ,通过 具有 延迟 环 
节 的 正 反馈 回路 形成 信号 模型 ,能够 获得 信号 中 的 各 种 谐 波 频率 成 分 。 

重复 控制 方法 的 出 现 , 为 伺服 系统 的 设计 及 解决 重复 轨迹 高 精度 跟踪 问题 提供 了 新 的 
手段 。 重 复 控制 方法 具有 较 强 的 实践 基础 ,而 且 工程 实现 简单 ,重复 控制 方法 在 高 精度 伺服 
系统 中 得 到 了 成 功 的 应 用 ,其 研究 成 果 在 机 器 人 控制 中 有 着 良好 的 应 用 前 景 。 


10.1.1 重复 控制 的 理论 基础 


重复 控制 方法 是 内 模 原 理 的 一 种 应 用 ,内 模 原理 是 指 : 如 果 控 制 系统 的 开 环 传递 函数 
包含 参考 信号 的 模型 ,那么 系统 闭环 输出 的 稳 态 误差 为 零 。 fa v 型 反馈 系统 跟踪 v 一 1 Br 


参考 输入 信号 无 稳 态 误差 ， 是 因为 其 开 环 传递 丽 数 中 包含 了 二 ,恰好 是 v 一 1 阶 输入 信号 


模型 。 
在 工业 应 用 中 ,时 常会 遇 到 指令 信号 或 扰动 信号 是 周期 的 例子 。 例 如 ,工业 机 器 人 固定 
的 运动 轨迹 ,可 以 看 作 是 在 周期 指令 信号 控制 下 完成 的 (如 CD-ROM 中 盘 片 不 规则 部 分 造 
成 周期 性 扰动 ) 。 
对 于 周期 性 指令 输入 或 干扰 ,如 果 将 周期 信号 的 产生 模型 引入 系统 闭环 中 ,根据 内 模 原 
理 , 便 可 实现 重复 控制 。 从 频 域 的 角度 来 看 ,重复 控制 方法 是 内 模 原理 的 一 种 应 用 , 适 于 跟 
踪 周 期 信号 或 抑制 周期 干扰 。 
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周期 信号 的 产生 模型 如 图 10-1 所 示 , 周期 信号 go 
R,(z) 通 过 一 个 纯 延 迟 环节 (延迟 工 秒 ) 构 成 正 反 馈 , 形 v 
成 周期 为 工 ,波形 如 有 cz) 的 周期 信号 。 


由 图 10-1 可 知 
RG)e^* -- R,G) —-RG) 图 10-1 周期 信号 发 生 器 
该 信号 模型 的 传递 函数 为 
R(s) ] 
Der i e Vt dd 


10.1.2 基本 的 重复 控制 系统 结构 


考虑 一 个 SISO 线性 时 不 变 系统 , 设 被 控 对 象 为 P(s)。 将 图 10-1 中 的 周期 信号 模型 串 
联 到 控制 回路 中 ,构成 基本 重复 控制 系统 ,如 图 10-2 所 示 。 


R(s) 
© 


10-2 基本 重复 控制 系统 


图 10-2 中 ,C(s) 为 基本 控制 絮 ,R(s) 为 周期 参考 信号 ,周期 为 L,n(s) 为 相同 周期 的 干 


ye R - CGOP G2) 
扰 信号。 通常 称 闭环 系统 jeevJPC) 为 基本 系统 。 


图 10-2 中 的 周期 信号 模型 的 前 向 通道 是 一 个 纯 延 迟 环 节 , 对 闭环 系统 特性 不 利 。 因 
此 ,如 果 给 周期 信号 模型 并 联 一 个 前 向 通道 ,其 上 串 比 例 环节 或 稳定 的 传递 函数 a(s), 则 有 
利于 改善 系统 的 稳定 性 和 快速 性 。 


10.1.3 基本 重复 控制 系统 稳定 性 分 析 


1. 基本 重复 控制 系统 的 等 效 系统 
图 10-2 中 ,基本 控制 系统 开 环 传递 函数 为 


W(s)=C(s)P(s) (10. 2) 
基本 闭环 系统 传递 函数 为 
co LORO 9,3 
则 
E(s)=R(s)—Y(s) (10. 4) 
Y(s)=W(s)U(s)+N(s) (10. 5) 
U(s) =E(s)+e™U(s) (10. 6) 
由 式 (10. 6) 得 
ECs 


Ue V 
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由 式 (10.4) 和 式 (10. 5) 得 
E(s)=R(s)—W(sU(s )— N(s) 


= Wu) -ES ws 
l= e” 
即 
1 
EG) (1-WG) 一 一 一 )=RG) 一 NG) (10.7) 
1 一 ee 
由 式 (10. 2) 和 式 (10. 3) 得 
sa WG) 
oem WO 
Bp 
G(s) 
WN a (10. 8) 
将 式 (10. 8) 代 入 式 (10.7) ,得 
E(s) 2(01—GG))( — e**) i (RG)—NGD (10. 9) 


1— 0—G(s))e* 
由 式 (10. 9) 可 得 到 图 10-2 的 等 效 系统 ,如 图 10-3 所 示 。 


R(s)-N(s) als) £s) E(s) 


图 10-3 ”基本 重复 控制 系统 的 等 效 系统 
其 中 ,图 10-3 中 的 第 1 个 回路 传递 函数 为 1 一 G(s), 第 2 个 回路 传递 函数 为 


一 CC, 故 可 看 出 图 10-3 为 式 (10.9) 的 示意 图 , 即 为 图 10-2 的 等 效 系统 。 

2. 小 增益 定理 的 描述 

定理 10.15 在 如 图 10-4 所 示 的 系统 中 ,A(s)、B(s) 都 是 稳定 的 ,但 所 组 成 的 闭环 系 
统 不 一 定 稳定 。 如 果 有 
SUP | A(jw) |e| BGe) | «x 1 (10. 10) 


OO «C wy « ox 


则 图 10-4 所 示 的 闭环 系统 稳定 。 

小 增益 定理 也 可 描述 为 :“ 如 果 闭 环 系统 的 开 环 增益 小 于 1, 则 对 应 有 界 输入 ,输出 
"3A. 

3. 基本 重复 控制 系统 稳定 性 证 明 

对 于 图 10-3 所 示 的 等 效 系统 ,应 用 小 增益 定理 ,可 得 
到 基本 重复 控制 系统 的 稳定 性 充分 条 件 。 

对 图 10-3 所 示 的 重复 控制 系统 so UH AE AR TE 


aa CGPC) 、 
(D GO= GF copo ERE. 


(2) SUP|1—G (jo)| <1. 图 10-4 闭环 系统 
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WA 10-2 所 示 的 基本 重复 控制 系统 稳定 。 

对 该 闭环 系统 稳定 性 分 析 如 下 , 首先 ,图 10-3 中 第 一 个 回路 传递 函数 为 1 一 G(s), 由 条 
件 (2) 可 知 , 第 一 个 回路 稳定 。 图 10-3 中 的 输入 R(s) 和 nn(s) 都 是 周期 为 L 的 有 界 周期 信 
号 .注意 1 一 e- 是 稳定 的 , Ie | 二 1。 图 10:3 中 第 二 项 传递 函数 为 1 一 er , 则 第 二 项 稳 
定 。 最 后 ,图 10-3 中 第 二 个 回路 传递 函数 为 LO O , 则 根据 小 增益 定理 ,有 

1—C€1—G(s))e 
SUP | AGoD | | B Go) |- SUP | 1 fel (1—GG))2e^* | 


=SUP | 1— G(s) |*| e |< 1 


则 第 二 个 回路 稳定 。 
通过 上 述 分 析 可 见 , 图 10-2 中 的 基本 重复 控制 系统 稳定 。 


10.1.4 仿真 实例 


被 控 对 象 为 
133 
s? + 25s 
采用 图 10-3 所 示 的 重复 控制 系统 结构 ,设计 基本 控制 器 为 
C(s) =s? -- 10s 
从 以 下 两 个 方面 证 明 所 设计 的 重复 控制 系统 的 稳定 性 。 
(1) 证 明 闭环 系统 G(s) 渐 进 稳定 。 


P(s)-— 


基本 控制 系统 开 环 传递 函数 为 
Wy -CtoPG) 2135 5 t 16 
s“ F255 

基本 闭环 系统 传递 函数 为 


W() 133s'J-4655s! 4-332505 _ 0. 99254s° (s 十 25)(s +10) 
1+W(s)  134s' 4- 47055? + 338755 s? (s 4- 25) Gs +10. 11) 
通过 闭环 系统 稳定 性 测试 程序 chaplO ltest. m 也 可 以 得 到 上 述 结论 。 由 上 式 的 极点 
分 布 可 知 ,基本 闭环 系统 G(s) 渐进 稳定 。 
(2) WEBISUPI1—G Ga) | —1. 
Ws) 1 
1+W(s) 1-W G2" 


G(s) = 


由 于 1 一 G(s)=1 则 


1 


Lbs dE ES i 


s 十 10s  134s4-1355 
ss: 十 25s  s+25 
_ | 134jw +1355 | _V(l134w) + 1355* 
| je +25 | Ja 4- 25* 
BISUP|1--W(Go)|21, JA MSUP|1—GGw)| <1. 
从 以 上 两 个 方面 分 析 可 见 , 所 设计 的 控制 系统 满足 重复 控制 的 稳定 条 件 (1) 和 (2), 即 所 


由 于 1 十 W(s)==1 十 133 , 则 


| 1--WG) |j >1 
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设计 的 重复 控制 系统 是 稳定 的 。 
指令 信号 是 频率 下 二 0.5Hz, 幅 值 为 0.5 的 正弦 信号 。 为 保证 LF 一 1, 重复 控制 回路 的 


延迟 时 间 为 L-6-2. 取 仿 真 时 间 为 20 ,仿真 结果 如 图 10-5 和 图 10-6 所 示 。 


—— ideal signal 
—-- practical signal 


0.6 
0.4 
2 
E 
s 02 
5 
= 0 
$ 
& 
-0.2 
—0.4 
0 2 4 6 8 10 12 14 16 18 20 
time(s) 
图 10-5 位 置 跟踪 
0.01 
5 0.005 
E 
o 
tb 
.号 
E 
S 0 
z 
Sg 
—0.005 
—0.01 
0 4 6 8 10 12 14 16 18 20 
time(s) 
10-6 位置 跟踪 误差 
仿真 程序 如 下 : 


(1) 闭环 系统 稳定 性 测试 程序 : chaplO ltest. m, 


* Repetitive Control for Servo System 


clear all;close all; 


P= tf([133],[1,25,0]); 
Cs t£([1100],[1]); 
W-C*P 

G-W/(1*W); 

zpk(G) 
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(2) Simulink 主 程序 : chaplO lsim. mdl, 


To Workspace2 


Transport 
Delay 


Clock To Workspace 


(3) 作 图 程序 : chaplO lplot. m, 


close all; 


figure(1); 

plot(t, y( :, 1), 'r' t, y( :, 2), 'b'); 
xlabel('time(s)');ylabel('position tracking'); 
legend('ideal signal', 'practical signal'); 


figure(2); 
plot(t,error(:,1), 'r'); 
xlabel('time(s)');ylabel('position tracking error'); 


10.2 一 种 具有 多 路 周期 指令 信号 的 数字 重复 控制 


将 重复 控制 方法 应 用 于 实际 伺服 系统 的 数字 控制 中 ” ,可 实现 伺服 系统 的 高 精度 控 
制 。 本 节 通 过 对 文献 L24 的 控制 方法 进行 详细 推导 及 仿真 分 析 ,研究 一 种 具有 多 路 周期 指令 
信号 的 数字 重复 控制 的 设计 、 分 析 及 仿真 方法 。 


10.2.1 系统 的 结构 


多 路 重复 控制 器 相当 于 在 控制 系统 的 闭环 回路 中 并 行 戏 入 了 多 个 重复 控制 回路 。 各 个 
重复 控制 回路 只 针对 特定 基 频 信号 进行 设计 ,因此 整个 重复 控制 系统 所 需 的 延 时 周期 要 远 
远 小 于 只 有 一 个 重复 控制 回路 的 系统 。 同 时 , 随 着 延 时 周期 的 减 小 ,调节 频率 加 快 ,误差 收 
敛 速度 提高 。 多 路 重复 控制 器 的 结构 如 图 10-7 所 示 。 

如 图 10-7 所 示 ,输入 指令 信号 或 扰动 信号 为 周期 信号 ,被 制 对 象 G(z ) 为 一 个 SISO 线 
性 离散 系统 : 

Y(z) —-G(2)U(z) 
其 中 ,G(z) 是 最 小 相位 系统 。 

设 采样 周期 为 ,重复 控制 回路 i 的 延迟 周期 频率 为 F; ,为 保证 N,F,T —1. EA d dl 

回路 i 的 延迟 周期 个 数 为 
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三 一 一 一 一 一 一 一 一 一 一 一 一 一 一 一 一 一 一 一 一 一 一 一 一 一 一 


Y(z) 


图 10-7 数字 多 路 重复 控制 系统 


1 -一 ... 
N =F T i =1,2, sn (10. 11) 
图 10-7 给 出 的 数字 重复 控制 系统 ,虚线 内 部 为 多 回路 重复 控制 器 ,可 表示 为 
Kc) 一 C:(z)( 27K， (10. 12) 


图 10-7 中 的 Ci (= ) 为 串联 补偿 环节 ,通过 设计 C1(z), 可 保证 由 G(z) 和 Ci(z) 构 成 的 反馈 
回路 为 渐进 稳定 的 最 小 相位 系统 : 


HC 这 Crin Gie) 
z)= 


 1+C,@)G(z) 


理论 上 ,存在 H GO B3 & 6 C, (GO ,使 下 式 成 立 : 
C,GOH(z)-21 (10. 14) 


X O0. 14) 是 控制 器 C,(z) 设 计 的 依据 。 


10.2.2 控制 器 的 设计 方法 


然而 ,在 实际 工程 中 ,存在 模型 不 确定 性 等 因素 ,因此 式 (10. 14) 应 表示 为 
C;GOH (z) —1-- A(z) (10, 15) 


(10. 13) 


其 中 ,|A(Cz)| 委 se,e 二 0。 
定理 10.2” 如 果 各 个 重复 控制 回路 的 增益 K, 满足 不 等 式 
Ko. Pelis Ho EST 


LHE 


(10. 16) 


其 中 ED。 
图 10-7 所 示 闭 环 系统 渐进 稳定 。 
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对 该 定理 的 稳定 性 分 析 如 下 : 图 10-7 所 示 的 数字 多 路 重复 控制 系统 中 ,闭环 系统 的 传 
递 函 数 为 
(14- K (2))C,(G2G Cz) 
1 4-C(G4- KGGD20C, G2G(Cz) 


(10. 17) 


G.(z)— 


由 式 (10. 13248 
H(z) 


C, GGG) = HG) (10. 18) 


将 式 (10.18) 代 入 式 (10.17) 式 中 ,并 将 式 (10. 15) 代 入 ,可 得 
H(z) 
Wer BUM T Ha H (zY(0 2- K 22) 


Gi Új uM BM l 
i 1— HG) -HG)0 - K() 


HODAK) | HG) - KG) 


1+ H(z)K G) n Z^ 
L4- HG C, DK; -— —- 
x i=l] 3 


n =N} 
HG) (1+ cc, 一 | 
NE LN. oma... (10, 19) 


= a 


14- (1+ AC »XK, — 
i=l ]—x- 


对 于 离散 系统 ,如 果 分 母 的 零点 在 单位 圆 之 内 , 则 系统 稳定 。 利 用 这 一 原理 ,通过 证 明 
RAO. 19) 分 母 所 有 零点 在 单位 圆 之 内 ,来 证 明 图 10-7 所 示 的 闭环 系统 渐进 稳定 。 
z—rcosg-t i * rsing , H) 


z“ =r cos( N,g) Hier" sin(N;ọ) 
g 1 1 
-N =s N == Ni N 2 
1—z ' z'—1 rz“ gos Ng) ier 'sin(N;9) —1 


r"cos(N,g) —l—i*r' sin(N,g) 
(r^ cos(N,g) — D? + Cr sin N92) 


- Ni 


AMARA e -K 的 实 部 为 


- Mi J- r` cos(CNip) 一 1] 
7751 QU eos No) — D J-€r ! sinCN;g)? 
p cos(N,p)—1 
p oy cos N;g) 4-1 
由 |z| 之 1 可 知 ,Ir| 之 1 PON 
207 cos( N;g) — 1) 之 一 ir Ap cos No) +1) 
即 
r'"cos(N,g) —1 1 
r ' —2r'cos(N;g) +1 2 
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也 即 


25 EX C10. 160 1 | AC | e , 则 有 


minRe| 92K e Mi= ] = min X, K, Re[z " /0— 2 )] 
dd cea} z|>1 ;=] 


* y lell 


之 一 a 
fesses 


即 
n - Ks 1 
: z /lz 一 和 一 | > 0， L 
wn DK is | * Ies > Y «sd 


于 是 可 得 
1 d =N; -N, " 
Irag) + OK /(1—z ')¥0, V|z|21 
反 过 来 ,可 得 最 终结 论 : 
1 
] 4- A(z) 
定理 10.3 若 图 10-7 所 示 的 闭环 系统 是 渐进 稳定 的 , 则 误差 渐进 收敛 到 零 。 
对 该 定理 的 稳定 性 分 析 如 下 ; 将 式 (10. 12) . 式 (10. 132 . 式 (10. 18) 和 式 (10.15) 代 入 ， 
则 图 10-7 所 示 的 闭环 系统 的 误差 传递 函数 为 
EG) 1 2 1 
R(z) 1-(G-K(G»DC,G)G) n 
1 (1c (22K 


HNMKz"/ü—-z")-0, V|z|«1 
i=l 


T(z)- 


z“ )) HG) 
1= H(z) 

TN C, (22GG) 

1= H(z) 1 二 Ci(z)G(z) 


—N 
l= * 


1- HG) (1c co( DK A) )uco 1-6 GHG[2)K 一 x) 
i=l i=l 


ES: ^ 一 之 


1 
Er s 
LE QE AG»(2K. x] 
i=l 


— 


(10. 20) 


由 式 (10. 20) RT UL, T C2 RAE H e) DEM G.(z) 分 母 相 乘 而 得 。 由 于 五 (=) 和 
G.(z) 是 渐进 稳定 的 , 即 H COM G (xz) 分 母 的 零点 都 在 单位 圆 内 ,从 而 T(z) 分 母 的 零点 
都 在 单位 圆 内 ,T(z ) 渐 进 稳定 。 

于 是 

| T(z)|=0, Vz'"21 
即 误差 渐进 收敛 到 零 。 
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10.2.3 仿真 实例 


假设 机 械 手 某 关 节 电 机 带 机 械 臂 的 模型 为 
8^ i 


iit C,GOX 
C, GO =s" + 100s 


则 
‘2 
CGU SC O6) e 155 5 1095 
s* -- 25s 
取 采 样 周 期 T —0. 001, 则 
33 一 119. 9z 
人 
1 — 0. 9753z 


根据 式 (10. 132 ,有 
C,G(z) X 133— 249.6z^! 十 116. 9 * 


Ha) 
14-C,G(z) 134 一 251.5z ! 4-117.9z * 
S 
| 2293 0. 8408—0. 84082 ! 
假设 模型 不 匹配 项 为 AC) — 一 一 1 ,将 其 离散 化 得 A(z) 王 一 一 一 一 一, 则 由 
s 1+0. 1581z 
lt 2751 


3X C10. 15518 
l1--A(z) 145.3— 262.8z ! 4-109. 2z * 4- 8. 72x ? 
H(z) 133 — 228. 6z ! -- 77. 45z +18. 48? 
指令 信号 由 频率 为 2Hz、2. 5Hz 和 4Hz 三 个 正弦 信和 号 组 成 。 则 根据 式 (10. 1 ,有 Ni 
500,N,—400.N,—250, RHEA (10. 16) ,各 个 重复 控制 回路 的 增益 取 K, =K, =K, = 
0. 15, 取 仿真 时 间 为 20s, 仿 真 结 果 如 图 10-8 和 图 10-9 所 示 , 取 仿真 时 间 为 120 ,仿真 结果 如 
图 10-10 所 示 。 


C;(z)-— 


3 r T T T T T 一 
一 一 ideal signal 
—— practical signal 
2 H 


position tracking 
o 


EL. 
8 10 12 14 16 
time(s) 


图 10-8 ”位置 跟踪 


e 
N 
A 
个 
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control input 


0 5 10 15 20 
time(s) 


10-9 仿真 时 间 为 20 的 位 置 跟踪 误差 


X103 


position tracking error 


0 20 40 60 80 100 120 
time(s) 


图 10-10 仿真 时 间 为 120 的 位 置 跟踪 误差 


仿真 程序 如 下 : 
(1) 初始 化 程序 : chap10_2int. m。 


% Repetitive Control for Multi - route Input 
clear all;close all; 
ts = 0.001; 


G= t£([133],[1,25,0]); 


C1 - tf([1 100 0], [1]); 
sys- Cl*G; 

dsys = c2d(sys,ts, 'z'); 

[num, den] = tfdata(dsys, 'v'); 
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H= sys/(1* sys); 
Hz = c2d(BH, ts, 'z'); 


zpk(Hz) % Zero point must be inside unit circle 


delta s- t£([1/2293 1], [1/2751,1]); 
delta z= c2d(delta s,ts, 'tustin') - 1; 


C2z= (1* delta z)/Hz; 
[numc, denc] = tfdata(C2z, 'v'); 


F1 = 2;N1 = 1/F1 * 1/ts; 
F2 = 2. 5; N2 = 1/F2 * 1/ts; 
F3 = 4; N3 = 1/F3 * 1/ts; 
€ N1 = 1/F1 * 1/ts = 500 
% N2 = 1/F2 * 1/ts = 400 
% N3 = 1/F3 * 1/ts = 250 


k1 = 0. 15;k2 = 0. 15;k3 = 0.15; 


zl = tf([1],[1 zeros(1,N1)],ts); 
z2=tf([1],[1 zeros(1,N2)],ts); 
z3 - t£([1],[1 zeros(1,N3)],ts); 


[numz1, denz1] = tfdata(z1, 'v'); 
[numz2, denz2] = tfdata(z2, 'v'); 
[numz3, denz3] = tfdata( z3, 'v'); 


(2) Simulink 主 程序 ; chap10_2sim. mdl, 


To Wotspace2 


Sine Wave2 Gain2 


To Workspaced4 


Qm e =] 


To Wotkspace3 


Sine Wave1 Gaini 


Clock To Workspace1 


其 中 ,Simulink 主 程序 中 的 控制 器 子 模块 如 下 : 
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Discrete 
Transfer Fcn5 


denz2 


Discrete 
Transfer Fong 


Discrete 
Transfer Fcn7 


Discrete 
Transfer Fon 


Discrete 
Transfer Fong 


In1 


(3) 作 图 程序 : chaplO 2plot. m, 


close all; 


figure(1); 

plot(t, y( :, 1), 'r', t, y( :,2),, 'b"); 
xlabel('time(s)');ylabel('position tracking'); 
legend('ideal signal', 'practical signal'); 


figure(2); 
plot(t,ut(:,1), 'r'); 
xlabel('time(s)');ylabel('control input'); 


figure(3); 
plot(t,e(:,1), 'z"); 
xlabel('time(s)');ylabel('position tracking error'); 
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机 械 手 容 销 控制 


CHAPTER 11 


在 控制 系统 中 , 当 系统 的 执行 器 发 生 故 障 时 ,传统 的 反馈 控制 设计 会 导致 不 满意 的 性 
能 ,甚至 整个 闭环 系统 失去 稳定 性 ,从 而 控制 系统 的 容错 控制 研究 得 到 了 广泛 的 重视 。 研 究 
控制 系统 的 容错 控制 具有 重要 意义 。 


11.1 执行 器 容错 的 输入 受 限 控制 


在 控制 系统 中 ,由 于 其 自身 的 物理 特性 而 引起 的 执行 机 构 输 出 幅 值 是 有 限 的 , 即 输入 受 
限 问题 ,由 于 控制 输入 受 限 的 存在 ,可 能 导致 整个 控制 系统 发 散 ,进而 导致 整个 控制 系统 失 
控 。 即 使 系统 不 发 散 ,长 时 间 高 强度 的 振荡 也 会 导致 控制 系统 的 结构 损坏 ,从 而 导致 故障 。 
所 以 ,控制 输入 受 限 控制 是 多 年 来 研究 的 热门 课题 。 本 节 考 虑 执行 器 故障 和 控制 输入 受 限 
同时 存在 下 的 控制 律 设计 。 


11.1.1 系统 描述 
针对 如 下 系统 
人 (11. DD 
z, =u +d 
其 中 ,zx, Mr, WIRKE did ACH uA deldi SD., 
执行 器 的 容错 控制 律 取 
u =v CT. 2) 


其 中 ,0 为 容错 系统 ,0 二 0, —0x1l.v 为 有 界 且 可 人 为 设 定 。 
控制 目标 为 : 四 设计 有 界 控制 律 v, 使 得 闭环 系统 内 所 有 信号 有 界 ; Qt 一 co 时 ,z, 一 0， 


a 0, 


11.1.2. 控制 器 的 设计 及 分 析 


1 -" ^ 
p5p p—b—p EEG coshr — 


e “十 ee 


Zzl.IlnCcoshzr)Z0. Hr —0 时 ， 
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InCcoshz) —0, Jy f iiEB]?24 tott, A 170 和 zy 一 0, 定 义 € 函数 为 


V =aln(cosh(kr, 十 lzz)) 十 Bln(cosh(CLzs Dd rH n 
Hm .a70,870,& 770.1770, 270. W] 
sinhe: Flza e nm 
a ie ia ETETE 


(11. 3) 


sinh(z4) , i 9 
cosh(/x;) “a Ted y ^? 
—a Gr; - [r,;)tanhGer; 4- 0x5) 4- BLr;tanh(Lr;) 4- £x; + zh 

= (al tanh (ez; 4- Lr;) 4- Bltanh (Ez?) + kx;)x; + akx;tanh(ex; + 1x) + E 

bb 


—(altanh(Ez,; +lx:) + Bltanh(Lz;) + kr,) (v ap 0 C.) 十 akzstanh(kzs zx PE E 
取 控 制 律 为 


=(altanh(kzxs+lz;)++Bltanh(lrs) 4- kx ;) (Ov +d) -- akx ,tanhCez; +t Lr,) +2 


v =p C— atanh(Ez, 十 人 zy) 一 Btanh(Lzs)) +v, (11.4) 
g= -F sgnal tanh (kz T d1x5;)-F BltanhGz;) 4- kx) 
L 


(11. 5) 
由 于 


— OCaltanh (ez; 十 lzs) 十 Btanh(CLz， 


)- Ex.) P sen (al tanh(Er, +x) + BltanhGx;) +kzx,) 
E 


Zr | eltanh (ex; 4- 4x5) 4- Bltanh Gaz) 4- kx; | 
L 


— OCaltanh(Ex; 4- L5) 4- Bltanh(x;) 4- &x;) d 


Bp 


OCaltanh(kzs + lx) + Bltanh(lzs) + kx) (v +ž) 
RARC(11 0, t =tanhlkx; +lx,),t; =tanh(lx ) 可 得 
0p Cal tanh (ez; o- 1x5) 4- Bltanh(Lz;) 4- &x;) CatanhGer; 十 lzs) 十 Btanh(CLzs)) + 


akx;tanh(Er; +lx:) + EL 


——60Cp +p) (alt， + Bit , +k) latz +Bti) +akzx:tz E 


— — (alt, t Bit, Fer) Cat, +e) — 6p (Gut; + flt, cr kx)(at; *&0- 1$) cr akzs;t; 
取 自 适应 律 为 


b — (alt, pitit kelate -- Br) 


306 «(]| 机 器 人 控制 系统 的 设计 与 MATLAB 仿 真 ， 基 本 设计 方法 (第 2 版) 


为 了 实现 输入 受 限 ,需要 保证 p 有 界 ,考虑 到 p 是 有 界 的 ,设计 投影 映射 算 子 , 取 
f — Proj; (1. 6) 
其 中 ， | 
0, PI p. HY(Glt; d- Blt, d- kx;) (at; +t) 之 0 
Proj; B4 Ê pus B. Y(Clt, 十 Bi kx) Cat, d- Bt, ) — 0 
isa det dediün Dos 其 他 


2 一 
|- E. - 


ete” 


由 于 0p-—l.xtanhr =x >0, M] x, tanh (lx) Z0. kx t: Z0 , W] 


V — — (alt; cr Blt , cr kry) Cat: T Bt.) -Fakzxt; 
=— i (at, gr» — kz: (at, T Bt) takrat, 
— — Lat; HA — kx ft, « 0 
M BAM at; - Bt, —0,2x5t,—0 时 ,V==0。 
考虑 tanhx 为 单调 递增 函数 ,由 于 rti — xr; tanh Ga) — 0, W] r, =0, Mmi ti — 0. Bl 
t;—0, 根据 1, — tanh Ge 4-4) . W kx,--ixr,—0,JÀTN ti =0. 
根据 LaSalle ^s 2E P CBE . AI RAA EREE . BD 24 17008 e 0.22790. HRAL DA 
RAL 5) ,控制 输入 幅 值 取 决 于 p。 a 、B。 


" -n 


E —8 


ge 


由 于 tanhx = &€[-] 41]. 


| v |=| C^ atanhCez, + lr) — Btanh (ar; )) n «Cp ua - BD tp 
因此 ,如 果 针 对 模型 式 (11. 1) 的 结构 , 按 式 (11.4) 设 计 控 制 律 ,并 按 式 (11.6) 设 计 自 适 
应 律 , 便 可 以 实现 控制 输入 的 受 限 。 


11.1.3 仿真 实例 


考虑 被 控 对 象 为 式 (11. 1) ,初始 状态 为 L0.5 0], 取 d= 二 3sint。 按 式 (11.4) 设 计 控 制 


律 ,考虑 执行 器 容错 范围 为 0.2 一 0 过 1, 则 1 委 p —5. C p. = L 9,5 HORT 6T E 
适应 控制 律 , 取 y —0. 10,a =10,8=10,k =10,1=10, W] 


| v | B... (a 十 及 十 天 一 5G10 十 10) 4-15 —115 
Lh 


AE u=0v,0 <01, M] |u| <115. 仿真 时 , 取 0=0.5。 在 控制 律 式 (11.5) 中 ,为 了 
消除 抖 振 ,采用 饱和 函数 方法 , 取 边 界 层 厚度 A 为 0.10。 仿真 结果 如 图 11-1 一 图 11-3 
所 示 o 


x response 


control input 
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time(s) 


图 11-1. 状态 响应 


20 40 60 80 100 


time(s) 


图 11-2 控制 输入 
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p estimation 


time(s) 


11-3 参数 p 及 其 自 适应 变化 


仿真 程序 如 下 : 
(1) Simulink 主 程序 : chapll l1sim. mdl。 


To Workspace1 


(2) 控制 器 程序 : chapll lctrl. m, 


function [sys,x0, str,ts] = spacemodel(t,x, u, flag) 
switch flag, 
case 0, 
[sys, x0, str, ts] = ndlInitializeSizes; 
case 3, 
sys = ndlOutputs(t, x,u); 
case (2,4,9) 
sys - []; 
otherwise 
error(['Unhandled flag = ',num2str(flag)]); 
end 
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function [sys, x0, str,ts] = mdlInitializeSizes 


Sizes = simsizes; 


sizes.NumContStates 
sizes.NumDiscStates  - 
sizes.NumOutputs = 
sizes. NumInputs = 
sizes. DirFeedthrough = 


Or uroo 
Ws 


sizes. NumSampleTimes 
sys = simsizes(sizes); 

x0 = [] 

str = []; 

ts = [E 

function sys » mdlOutputs(t, x, u) 
x1-7u(1); 

x2 = u(2); 

p1 = u(3); 


alfa = 10;beta = 10;k-71.0;172 1.0; 
D= 3.0} 
thetaL = 0.20; 


s-((1*(alfa*tanh(k* x1 + 1 * x2) + beta * tanh(1* x2)) * k * x2)); 
€ vs- - D/thetaL * sign(s); 
fai- 0.10; 
if abs(s)« -» fai 
sat s- s/fai; 
else 
sat s-7 sign(s); 
end 
vs= - D/thetaL* sat s; 
%vs= 0; 
vt-pl*(-alfa*tanh(k* x1 + 1 * x2) - beta * tanh(1 * x2)) * vs; 
sys(1) = vt; 


(3) 被 控 对 象 程序 : chapl llplant. m. 


function [sys, x0, str,ts] = s function(t, x, u, f lag) 
switch flag, 
case 0, 
[sys, x0, str, ts] = mdlInitializeSizes; 
case 1, 
sys = ndlDerivatives(t, x,u); 
case 3, 
sys = ndlOutputs(t,x,u); 
case (2, 4, 9) 


sys = []; 
otherwise 

error(['Unhandled flag = ',num2str(flag)]); 
end 


function [ sys, x0, str, ts] = mdlInitializeSizes 
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Sizes = simsizes; 


sizes. NumContStates 


sizes.NumDiscStates 


sizes.NumOutputs 


sizes.NumInputs 


LU 
oOo o n-o t o t 
S. 


sizes. DirFeedthrough 


sizes.NumSampleTimes 
Sys 7 sinsizes(sizes); 
x0 [0.50]; 

str -[]; 

ts -[IL 

function sys = mdlDerivatives(t, x, u) 
vt-u(1); 

theta = 0.5; 

ut = theta * vt; 

d-3»*sin(t); 

sys(1) = x(2); 

sys(2) - ut +d; 

function sys = mdlOutputs(t, x, u) 
sys(1) = x(1); 

sys(2) = x(2); 


(4) 参数 估计 程序 : chapll 1p. m. 


function [sys, x0, str,ts] = NDO(t, x,u, flag) 
switch flag, 
case 0, 

[sys, x0, str, ts] = mdlInitializeSizes; 
case 1, 

sys = ndlDerivatives(t,x,u); 
case 3, 

sys = ndlOutputs(t,x,u); 
case (2, 4, 9 ) 


sys = []; 
otherwise 

error(['Unhandled flag = ',num2str(flag)]); 
end 


function [sys, x0, str, ts] = mdlInitializeSizes 


Sizes = simsizes; 


sizes.NumContStates 


Li 


sizes.NumDiscStates 


sizes.NumOutputs 
sizes.NumInputs 
sizes.DirFeedthrough 


ii 
ornnbeokrH 


sizes. NumSampleTimes 
sys = simsizes( sizes); 
x0 -[0]; 

str -[]; 

ts =[]; 
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function sys = mdlDerivatives(t, x,u) 

x17u(1); 

x2 = u(2); 

alfa = 10;beta = 10;k = 1.0;1= 1.0; 

gama = 0.10; 

t2 = tanh(k * x1 + 1* x2); 

tl = tanh(1 * x2); 

dp = gama * (alfa * 1 * t2 + beta * 1* t1 +k * x2) * (alfa * t2 + beta * t1); 


p=x(1); 
pmin = 1;pmax= 5; 


if p» = pmax&dp> 0 


sys(1) = 0; 

elseif p<= pmin&dp < 0 
sys(1) = 0; 

else 

sys(1) = dp; 

end 


function sys = mdlOutputs(t, x, u) 
p7x(1); 
sys(1) = p; 


(5) 作 图 程序 : chapll_lplot. m, 


close all; 


figure(1); 
plot(t,x(:;,1),"k'?t,x(:,2), 'r:',. "] inew1dEh', 2) ; 
xlabel('time(s)');ylabel('x response'); 
figure(2); 

plot(t,v(:,1), 'k', 'linewidth',2); 
xlabel('time(s)');ylabel('control input'); 
figure(3); 


plot(t,p(:,1),'- .b', 'linewidth',2); 
xlabel('time(s)');ylabel('p estimation'); 


11.2 传感器 和 执行 器 同时 容错 的 反 演 自 适应 控制 


11.2.1 系统 描述 
考虑 如 下 系统 : 


(11. 7) 


传感器 和 执行 器 的 容错 取 
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zip; i=1,2 (11. 8) 
u —p,v (11.9) 
其 中 ,d 为 加 在 输入 上 的 扰动 , |d |D, po We. J JR AR s R, 0< pio S p; S1, 0 po, S 


PSl. 

输出 x, 指令 为 y, ,传感器 实测 输出 为 xy 和 xs。 控 制 目标 为 : DuibfzmüG v, (E19 
闭环 系统 内 所 有 信号 有 界 ; Otok}, ri > yati Yao 

11.2.2 控制 器 的 设计 与 分 析 


采用 反 演 控制 算法 设计 控制 律 。 
(1) 定义 z,—xl1—y452; =x] —a1— y, WU 


£i —piX2 一 yd 


Z2 =“P2%2 — QI 一 ya 
设计 如 下 Lyapunov PX: 


1 
Y. 
则 
V, —z,£i —ziCpix; =a) 
j' A 
由 于 zs 一 二 (zz 十 qr 十 ya), 则 
2 


V =z, MZ te 30 3a) 
2 


1 
由 于 0 p, Sp, X1 , 则 £20 E HS 
1 2 


20 


" 1 fpi ) l ER 
si E — — < —— -Z 
T T 2 (Ez, + 277? =EN cT 2 
定义 a4 — cizi 0,270, Bill 
; à 1 1 
V, =z, (e en 十) azi S pr abs aht + 
p2 20:0 2 ez 


-(d--681)4ela-f-o: TERRENT 
ES E zi tgz t tr 十 可 站 
j! 


ji pı 
^ \2p% — Cipio 20; EZ z 


reithe) 


1 
205 
Sdn Ett. 
p2 
(2) 由 定义 可 得 
之 2 一 zzZz Eo (一 cizl) 一 yd —ps;Cu +a) 十 ci 之 i 一 yd =p: (pov +d) +c lpr: 一 y4) 一 yd 
设计 如 下 Lyapunov 函数 


=V, 十 d 
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1 1 
由 于 pw 所 全 二 一 ,z= 二 x?, 则 
e 220 p2 
V, =V, + zi. 
s 1 í A A $ ls 1 ] T 
< ( gp Pù 2 二 二 
lows wy 
z2 (psp v 十 pzd Tc: (o: ps —34) X) 
— ERES ER EET 
Hizi 2 2 2N42 p2 2 2 1a 2 205, Na 2 2 2 d 


" 3 > 
<— uzi tun +z: (uv +p:d + 4x3) +C, 


1 Eu Ww MOM a a a d 
py un T ppod 7a Pc TS Caso c (zz) 


其 中 Hl MENSES 2 2p20 pz 20», 


Tus 
7i» lond | D. 


BT $c, 站 为 未 知 常数 ,需要 采用 自 适应 方法 ， 
设计 Lyapunov 函数 如 下 
V=V; tud 
其 中 ,让 一 多 一 上 ,7 过 0。 
则 
m ANS 交角 sC— uix! 十 可 T z;Qiv +p:d 十 gr) 十 Ci 十 ; 
由 于 ys 二 pspo 未 知 ,设计 控制 律 为 


a—k,z--$ri —y.--gsgnz;; kı 0, q>en +D (11.10) 
v ——h,z,—a (11.11) 
v=N(k)D (11. 12) 
k=7z0, Yi>0 (11. 13) 


V <— pz? 十 可 二 (a — (kiz 十 gz —ys 二 nsgnzs) 十 LoN(k)v 二 pzd Tx 


3 z " " 
-——Hnzi +77: 二 ala —kizz — $r} 十 yd — 3sgnz; + u; N Ck)v +p:d) + 


$5- T zC— krzy =a) 
2 


= ns (11.14) 
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l: E 
由 元 《一 zs M 
G 3 " E E 
V Sazi Hg nu C kiz tya — qsgnz: + po NOOv pid) +C, — b +z: C- kaza) 
' 2 
2 3 2 i Ll 、 ls 2 
和 uz Tu — kizs zoya p; NCR) ya T C; B = as 
2 2 


3 
S— nt e (= 


a 2 latis ee 1; 
一 b.) ei zr XR NOD HEC  - v 


=— puzi — (ki Hkz — 2)2i +C: taNo hk 
2 2 
l ls 
—AV FAN CR) —h — Ga 
Ya Y2 


1.4 
其 中 „C=C, 5 yi A = minty, ski tke 2ka 


HF oue < JC yd 一 sint, 则 


C imas = -7 E 十 
根据 引 理 11. 2, 可 得 


RE RE. = TtT 


VG) « e™V(t,) 和 | ea NO) > = "Dd: «| Ce de 
即 
—At —at 1 ^ - Camax 一 好 
VESLE VO FE N (u, NCR) — Dke dct (le ) 
2* fg 
Lete vOe” =f (ua N(k)—1)ke"dr 
G max 
其 中 eo 一 T o 


则 根据 引 理 11. 17 ,VCO YE Vr € [0.00 EAR, MAM zz; FW TE VLC [0.70 Ef 
Ns 通过 取 足 够 大 的 4 和 y; 值 ， 可 保证 t—oo9HB[ .z.. 之 2 足够 小 ， 从 而 ri *yaíml— Yao 


11.2.3 仿真 实例 
考虑 模型 式 (11. 7) ,指令 取 ya= sint , 取 d — sinzt spo 770. 50.0; =0, 95,0; --0, 95, 
1 1 1 
为 满足 À — mini Hiski Hk —2}>0, 4 = i Ri 一 4， 
k,—1,c,-—20, 


根据 定义 11. 1, N (k) = k’ cosk ,采用 控制 律 式 (11. 10) 一 式 (11. 13) 和 自 适应 律 
XOL1D.JÉEy,—y,—1.0.5-D--0.10—1.1, É& sk C11. PROS 1.0, Ba 
应 律 式 (11. 14) 中 的 初 值 取 0. 10, 

为 了 防止 拌 振 ,控制 器 中 采用 饱和 函数 satz, 代替 符号 函数 sgnz,, 即 
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l, £5 > A 
PS | zs [E.M IA 
-— ME Lour 
其 中 ,A 为 边界 层 , 取 A-—0.05, 
仿真 结果 如 图 11-4 和 图 11-5 所 示 。 


—— ideal angle signal 


E] — 
5 sensor output x1F j 
p = * = plant angle output x1 
E 
9 w 
& 
0 5 10 15 20 
time(s) 


= ideal angle speed signal 
—7 sensor output x2F 
== * = plant angle speed output x2 


speed tracking 


0 5 10 15 20 
time(s) 


图 11-4 角度 .角速度 响应 


400 
300 
100 
0 
-100 

-2003 5 10 15 20 


time(s) 


N 
e 
e 


control input 


11-5 控制 输入 
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仿真 程序 如 下 : 
(1) Simulink 主 程序 : chapll 2sim. mdl。 


chapí1 2plant 


S-Functiont 


To Workspace2 


chap11_2ctf 
S-Function 


To Workspace 


(2) 输入 指令 S 函数 : chapll 2input. m, 


function [sys,x0, str,ts] = spacemodel(t,x, uu, flag) 
switch flag, 
case 0, 
[sys, x0, str, ts] = mdlInitializeSizes; 
case 3, 
sys = mdlOutputs(t, x,u); 
case (2,4,9) 
sys- []; 
otherwise 
error(['Unhandled flag = ',num2str(flag)]); 
end 


function [sys, x0, str,ts] = mdlInitializeSizes 
sizes = simsizes; 


F^ oor-—00 
M 


sizes.NumContStates 
sizes.NumDiscStates  - 
sizes. NumOutputs = 
sizes.NumInputs - 
sizes.DirFeedthrough - 
sizes.NumSampleTimes = 
Sys = simsizes(sizes); 


x0 -[] 
str = []; 
ts - [00]; 


function sys = mdlOutputs(t, x,u) 
sys(1) » sin(t); 


(3) 控制 器 S 函数 : chapll 2ctrl. m, 


function [sys,x0,str,ts] = spacemodel(t,x,u, flag) 
switch flag, 
case 0, 
[sys, x0, str, ts] = ndlInitializeSizes; 
case 1, 
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sys = ndlDerivatives(t,x,u); 
case 3, 
sys = mdlOutputs(t,x,u); 
case (2,4,9) 
sys- []; 
otherwise 
error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0, str, ts] = ndlInitializeSizes 


sizes = simsizes; 


sizes.NumContStates 
sizes.NumDiscStates  - 


2 
0 
sizes.NumOutputs = 1; 
sizes.NumInputs = 3 
sizes.DirFeedthrough = 1 
sizes.NumSampleTimes - 1 

sys = simsizes(sizes); 

x0 -[0.11.0]; 

str - []; 

ts [0 0]; 

function sys = ndlDerivatives(t, x,u) 
yd= u(1);dyd-» cos(t);ddyd- - sin(t); 
xl = u(2);x2 = u(3); 


roul = 0. 95; rou2 = 0.95; 
XxlF = roul * x1;x2F = rou2 * x2; 


cl = 20; 

zl = x1F - yd; 

alfal = ~ cl * z1; 

z2 = x2F - alfal - dyd; 


faip=x(1); 
k1 = 4;k2= 1; 
xite= 0.10; 


alfa = kl * z2 + faip * x2F - ddyd + xite * sign(z2); 
vb= -k2* z2- alfa; 


gamal = 1.0; 
dfai = gamal * z2 * x2F; 


gama2 = 1.0; 

dk = gama2 * z2 * vb; 

sys(1) = dfai; 

sys(2) = dk; 

function sys = mdlOutputs(t, x, u) 

yd 7 u(1);dyd-» cos(t);ddyd- - sin(t); 
xl = u(2);x2 = u(3); 


roul = 0. 95; rou2 = 0.95; 
x1F = roul * x1;x2F = rou2 * x2; 
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cl = 20; 

Zl = xlF- yd; 

alfal = -cl*zl; 

z2 = x2F - alfal - dyd; : 


k1 = 4;k2= 1; 
D=1.0; 
xite= D+ 0.10; 


delta = 0. 05; 
M = 1/delta; 


if abs(z2)> delta 
satz2 = sign( z2); 


else 

Satz2 = M * z2; 
end 
faip= x(1); 


alfa = kl * z2 + faip * x2F — ddyd + xite * satz2; 


vb= -k2 * z2- alfa; 
k2x(2); 

Nk = k^2 * cos(k); 
vt = Nk * vb; 


rou0 = 0.50; 
ut = rou0 * vt; 


sys(1) = ut; 
(4) 被 控 对 象 S ERG: chapll 2plant. m. 


function [sys, x0, str,ts] = spacemodel(t, x,u, flag) 
switch flag, 
case 0, 

[sys, x0, str, ts] = mdlInitializeSizes; 
case 1, 

sys = ndlDerivatives(t,x,u); 
case 3, 

sys = ndlOutputs(t,x,u); 
case (2,4,9) 

sys - []; 
otherwise 

error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0, str, ts] = mndlInitializeSizes 
Sizes = simsizes; 
sizes.NumContStates  - 


2 
sizes.NumDiscStates  - 0; 
sizes.NumOutputs = 2 

1 


sizes. NumInputs - 
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sizes.DirFeedthrough 


LU 
H 


sizes.NumSampleTimes = 1; 

sys = simsizes(sizes); 

x0 = [0.20;0]; 

str = []; 

ts = [00]; 

function sys = mdlDerivatives(t, x,u) 
ut=u(1); 

dt = sin(pi*t); 


sys(1) =x(2); 

sys(2) = ut + dt; 

function sys = mdlOutputs(t, x, u) 
sys(1) = x(1); 

sys(2) = x(2); 


(5) 作 图 程序 : chapll 2plot. m, 


close all; 


roul = 0.95;rou27 0.95; 
xlF = roul * y(:,2);x2F = rou2 * y(:,3); 


figure(1); 

subplot(211); 

plot(t,sin(t), 'k', t, x1E(:,1), 'r', t, y( :,2), ' - . b', 'linewidth',2); 

xlabel('time(s)'); ylabel('position tracking'); 

legend('ideal angle signal', 'sensor output xlF', plant angle output x1'); 
subplot(212); 

plot(t,cos(t), 'k', t, x2F( :, 1), '£', t, y( :,3) ,' — . b', 'Linewidth',2); 
xlabel('time(s)');ylabel('speed tracking'); 

legend('ideal angle speed signal', 'sensor output x2F', 'plant angle speed output x2'); 


figure(2); 
plot(t,ut(:,1), 'k', 'linewidth',2); 
xlabel('time(s)');ylabel('control input'); 


11.3 传感器 和 执行 器 同时 容错 的 RBF 网 络 输入 受 限 控制 


11.3.1 系统 描述 
针对 如 下 系统 


i,—ucfG, a) (11.15) 
其 中 ,zk 和 zs 为 状态 ,u 为 控制 输入 ,f(x r O ARAMA R RR 
传感器 实测 输出 为 x 和 xz 一 Tizry1<7<1 一 1,2。 控制 目标 为 : 四 设计 有 界 
控制 律 ,使 得 闭环 系统 内 所 有 信号 有 界 ; OODz 一 ce 时 ,zi 一 0,zy 一 0。 
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11.3.2 RBF 神经 网 络 逼近 
采用 RBF 网 络 可 实现 未 知 函 数 f Goo fal m. RBF 网 络 算法 为 
f 2W'hG) +e 
其 中 ,x 为 网 络 的 输入 ,hh 二 [hi shah, 为 高 斯 函数 的 输出 ,W 为 网 络 的 理想 权 值 ,s 为 
FI GREASE lel e, W—[W, W; - W,] Waon SIW, Wu sj 1.2... 
采用 RBF 逼近 未 知 函数 f Gri n HUP. 19, —1 为 未 知 常数 , 故 网 络 输入 可 取 x 一 
[rf xi], W RBF 网 络 的 输出 为 


f(x) —W'h) (11. 16) 
DU 
Fix) 9 fG) —fG) =W"h (x) +e —W'h (x) —W'ho +e 
JE SLW—W—-W, 
由 于 未 知 函 数 f(z) 有 界 , 则 f(z) 有 界 , 可 令 |f AOS fao 


11.3.3 控制 器 的 设计 及 分 析 


设计 辅助 控制 律 为 
v — —atanh(ez] + 1x2) — Btanh(Lx?) (11. 17) 
K'rhari—gzxoi—1,2,1«59,X€10a..k.1070, 
取 控 制 律 为 
w= wf (11. 18) 
其 中 , f(z) 为 RBF 网 络 对 函数 f£ Goo HU B EXER 
则 模型 变 为 
Qux. 


i,-— —atanh(ezT + 1x2) — Btanh xi) tf (x) 


Ep, F= fafa). 
定义 Lyapunov AŠ 


V —aln(cosh(Ex* 4- 1x52) + BlnCcosh (4x 0) + zWW (11.19) 
HRB.y0,. 
则 
， sinh(CAzT + x1) A sinh(/z$), . "noo 
让 
* oshik! 4- 1x) Ts is AETAT, "s Y 


Pew 


—ayn(kz; 4- l1z;)tanh(kz] o- 1x2) + Blgz;tanh(Lz?) — T 
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A t, —tanh(ez T tlr) 4, — tanh G3) lll] z, — —at,— fts +f) M ERTS Ag 


V —ag(z; +1(— at, — Bt; +f Gt, 4- BlgC— at, — Bt; +f G0, — IW'W 
—aTnkx st, — xl Kati c 2a Bt ,t, - gii er 9g QW* h GO -F e) Calt, 十 Blt,) ue Tarw 
=ankx:tı — ql Catı +t)? + 3W hx) Colt, + Blt;) + ye lalt, tg — LW'W 


—apkz,t, — ql Cat, d- Bt)! + iW" C (x Colt, 十 Bt) 一 z% 十 大 (alt + Blt,) 


OH— RERO VENE E ERE HAW — yh GO L Cat HB) y >0. 由 于 f(x) 有 界 , 为 
了 实现 该 函数 的 有 界 允 近 , 采 用 神经 网 络 有 界 映射 自 适 应 律 。 首 先 设计 投影 映射 算 子 ,考虑 


W—W-—W,5E X.€ —h (x) (alti 十 Blts), 为 了 保证 WT (e -;w)«o. n Waa < IW, | « 


W ax o B 
W, =yProj, (€,) 
w 
其 中 ， 
p W, zx M uas HE, > 0 
Proj, (£,) = à 11. 20 
roja (8,) 0, W, K W us HE, zo ( ) 
£&. 其 他 
取 Proj C£ ) — (Proj, C£,0) , 则 有 界 映射 自 适应 律 为 
w w 
W — yproj. (£ ) (11.21) 
w 


采用 自 适 应 律 式 (11. 20) ,可 保证 
pus 1 . 
Ww! (e— yProi. CE ) <0 
则 


V x— Cot, -F gr,» — kBx;t; -F € Calt, T Blt;) 


由 于 ztanhz 一 > 二 -一 之 0, 则 z,tanh (x20, W] kBzsts 宇 0, 从 而 
© & 


V «C— lat, 4- Bt; Y + el Cat, + fta) 
— — Cat, +t)? 4- el Cot, + Bt; -Lle ue 


1 
<—i((at RO — e) Ade 


由 于 tanhz 2 UE H5 R, A IE. 2, — tanh Ger, - 1x2) 4t; — tanh (x) ,a 48790, B Cat, 十 
Bts) 也 为 单调 递增 函数 ,因此 对 于 任意 的 870 ,存在 一 个 有 限时 间 25.24 lati Het | 0. E 
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得 V0 成 立 。 因 此 x, 和 > 在 有 限时 间 内 收敛 到 半径 为 6 的 紧 集 内 ,并 且 保 持 在 该 紧 集 
内 ,系统 的 收敛 速度 取决 于 a,B,k ,1。 


和 he 十 1], 则 可 得 控制 输入 幅 值 为 
| u |-|— atanh(ex  4- Lx5) — Btanh(lz;) fy |t a BH as 


因此 ,如果 针对 模型 式 (11.15) 的 结构 , 按 式 (11. 18) 设 计 控 制 律 ,并 按 式 (11. 21) 设 计 自 
适应 律 , 便 可 以 实现 控制 输入 的 受 限 。 


11.3.4 仿真 实例 


考虑 被 控 对 象 为 式 (11. 15) ,初始 状态 为 L0.5,0]。 


RBF 网 络 采用 5 个 隐 含 层 节点 , 即 n= 二 5, 则 fa G0 -W'hRGO IW Il à GO | 去 
5W。 根 据 网 络 输入 x, 的 实际 范围 设计 高 斯 基 函 数 的 参数 ,参数 c, EM 取 值 分 别 为 
[一 1 一 0.5 0 0.5 1j 和 3.0。 网 络 权 值 中 各 个 元 素 的 初始 值 取 0. 10, 

按 式 (11. 21) 设 计 自 适应 律 , 取 W,, 二 一 1.0,W 二 1.0,7==0. 10, 按 式 (11. 18) 设 计 控 
l8 Ca —10.8—10.4 —10,7— 10. Bil 


| u |=|— atanh(Gez, +lx:)— Btanh(lz,) —fG) | 


过 w 十 有 + 一 1 十 10 十 5 一 站 
仿真 结果 如 图 11-6 一 图 11-8 所 示 。 


3— 


states response 


states x response 


time(s) 
图 11-6 状态 响应 
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N 


L 


小 


control input 


0 5 10 15 20 
time(s) 


11-7 控制 输入 


fx and its estimation 


0 5 10 15 20 
time(s) 
图 11-8 jxr) 及 其 逼近 效果 


仿真 程序 如 下 : 
(1) Simulink 主 程序 ; chapll 3sim. mdl。 


chap11 3plant 


To Workspace? 


Clock To Workspace 
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(2) 控制 器 程序 ; chapll 3ctrl. m, 


function [sys,x0, str,ts] = spacemodel(t, x,u, flag) 
switch flag, 
case 0, ‘ 
[sys, x0, str, ts] = mdlInitializeSizes; 
case 1, 
sys = ndlDerivatives(t,x,u); 
case 3, 
sys = mdlOutputs(t, x, u); 
case (2,4,9) 
sys- []; 
otherwise 
error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0, str, ts] = ndlInitializeSizes 
global bj cij 
cijs0.5*[-2 -1012; 
-2 -401 2]; 
bj=3.0; 


sizes = simsizes; 


sizes. NumContStates 


EE 


sizes.NumDiscStates 


~s 


sizes.NumOutputs 


sizes.NumInputs 
sizes.DirFeedthrough 
sizes. NumSampleTimes 


uo dw 
Ni "We 


LU 
Oor tw N Oo wm 
No 


M. 


SyS 7 simsizes(sizes); 


x0 -2[0.10.10.10.10.1]; 
str = []; 
ts = [k 


function sys = mdlDerivatives(t,x,u) 
global bj cij 


xl = u(1);x2 = u(2); 
xite= 0.80; 

xlF = xite * x1; 
x2F - xite * x2; 


xi= [xlF;x2F]; 


h= zeros(5,1); 
for j= 1:1:5 

h(j) = exp( - norn(xi - cij(:,3))^2/(2 * bj^2)); 
end 


alfa = 10;beta = 10;k = 10;1 = 10; 
t1 = tanh(k * x1F + 1 * x2F); 
t2 = tanh(1 * x2F); 
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gama= 0.10; 


W= [x(1) x(2) x(3) x(4) x(5)]; 


Wmin- - 1;Wmax= 1; 
for j=1:1:5 

M= 2; 

if M== 


dw(j) = gama * h(j) * 1 * (alfa * t1 + beta * t2); 
sys(3) = dw(j); 
end 


if M== 
Kesi(j)=h(j)*1x(alfaxtl+betaxt2); 
dw(j) = gama * Kesi(j); 

if W(j)» = Wnax&Kesi(j)» 0 


sys(3) 7 0; 

elseif W(j)« » Wnin&Kesi(j)« O 
sys(3) ^ 0; 

else 

sys(3) = dw(3); 

end 

end 

end 


function sys = ndlOutputs(t, x, u) 
global bj cij 


x17u(1);x2-7 u(2); 
xite = 0.80; 

xlF- xite * x1; 
x2F = xite * x2; 


xi= [x1F;x2F]; 


W= [x(1) x(2) x(3) x(4) x(5)]; 
xi = [x1;x2]; 


h= zeros(5,1); 
for j21:1:5 
h(j) = exp( = norn(xi - cij(:,3j))^2/(2 * bj^2)); 
end 
fp-W*h; 


alfa = 10;beta = 10; 
k-710;1-7 10; 


tl = tanh(k * x1F + 1 * x2F); 
t2 = tanh(1 x x2F); 


ut- —alfa*tl- beta * t2 - fp; 
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sys(1) = ut; * Umax = alfa + beta + fp max + ddxd max) =10+10+4+1=25 
sys(2) = fp; 


(3) 被 控 对 象 程序 : chap11_3plant. m, 


function [sys, x0, str, ts] = s function(t,x,u, flag) 
switch flag, 
case 0, 

[sys, x0, str, ts] = mdlInitializeSizes; 
case 1, 

sys = ndlDerivatives(t,x,u); 
case 3, 

sys = mdlOutputs(t, x, u); 
case (2, 4, 9} 

sys = []; 
otherwise 

error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0,str,ts] » mdlInitializeSizes 
sizes = simsizes; 
sizes.NumContStates  - 
sizes.NumDiscStates  - 
sizes. NumOutputs = 
sizes. NumInputs = 


sizes.DirFeedthrough = 


oO Ot t O t 
二 


sizes.NumSampleTimes = 
SyS 7 simsizes(sizes); 
x0 [0.5 0]; 

str =[]; 

ts sl 

function sys = mdlDerivatives(t, x, u) 
ut- u(1); 

fx= 5 * tanh(x(2)); 

sys(1) = x(2); 

sys(2) = ut- fx; 


function sys = mdlOutputs(t, x, u) 
fx=5* tanh(x(2)); 


sys(1) = x(1); 
sys(2) = x(2); 
sys(3) = fx; 


(4) 作 图 程序 : chapll 3plot. m, 


close all; 


figure(1); 

plot(t,x( :, 1), 'k', t, x(:,2),, 'r:*, linewidth", 2); 
legend('states response'); 
xlabel('time(s)');ylabel('states x response'); 


figure(2); 
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- 


plot(t,ut(:,1), 'k', 'linewidth',2); 
xlabel('time(s)');ylabel('control input'); 


figure(3); 

plot(t,x(:,3),'r',t,ut(:,2),' — .b*, 'Tinewidth',2); 
xlabel('time(s)');ylabel('fx and its estimation'); 
legend('fx', 'fxp'); 


附录 


定义 11.1. 如 果 函 数 N(X) 满 足下 面条 件 , 则 NOD Nussbaum 函数 。Nussbaum 
函数 满足 如 下 双边 特性 


k 
lim sup zl N (s)ds — oo 
ket 9 k 0 


k 
lim inf 二 | N(s)ds =— 2 


keta 

根据 Nussbaum Ag Æ , Æ 3 Nussbaum 函数 为 

N (k) =k" cos(k) 

其 中 ,k 为 实数 。 

31 11.1 如 果 V(1) 和 k(，) 在 V1E[0,tj) 上 为 光滑 函数 ,V(1) 宇 0,N(，) 为 光 
He ON HK .0, 为 非 替 常数 ,如 果 满 足 

VG) L co e l'a, coN GG» — lk(r)e' dr, Vt € [0,,) 

XcBP.c,20.c, 为 常数 ,gb,(t) 为 未 知 时 变 参 数 。 


则 VC GD 和 | G0, GONG G) 一 DCr)dr 在 ViE[o,tr) 上 有 界 。 


引 理 11.27. 不等式 方程 V 委 一 cV 十 ,Vi 三 in 三 0 的 解 为 


—a(r—t,) 


VG) s e" v(,) +f e"? f (ode 
其 中 ,a 为 任意 常数 。 
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基于 事件 驱动 的 机 械 手 
反 演 控制 


CHAPTER 12 


随 着 通信 技术 和 传感器 技术 的 发 展 ,20 世纪 90 年 代 出 现 了 事件 驱动 控制 的 概念 。 事 
件 驱动 控制 的 基本 思想 是 基于 测量 信号 的 通信 数据 只 有 当 事 件 驱 动 策略 的 设计 条 件 得 到 满 
足 时 才 会 被 发 送 。 在 事件 驱动 中 , 当 某 个 事件 (通常 是 一 组 策略 、 函 数 、 算 法 或 条 件 ) 超 过 给 
定 阅 值 时 ,就 执行 采样 或 更 新 控制 输入 。 通 过 采用 事件 驱动 策略 ,可 以 大 大 节省 信号 的 通信 
Eo 事件 驱动 控制 理论 目前 是 网 络 控 制 理论 研究 的 热点 。 


12.1 基于 固定 阅 值 的 事件 驱动 反 演 控制 


12.1.1 基本 原理 
被 控 对 象 为 


X, =T] 
(12. 1) 
Ta =f (x,t) c-g.,t)u 


其 中 ,g(x,t) 关 0。 
定义 xz! 为 角度 ,x, 为 角速度 ,定义 角度 误差 zx; 一 zi 一 zid, 其 中 ,zs 为 指令 信号 , 则 


Z1 一 1 一 Zd 一 并 2 一 之 d 


控制 目标 是 tokt, r za M ag lo 


12.1.2. 控制 器 的 设计 与 分 析 


采用 反 演 控制 方法 设计 控制 律 ,设计 步骤 为 : 
(1) 定义 Lyapunov PR 


则 


V,—z£,—z.G, za) 
取 r, =— cz] 2E tze ,其 中 rore vato 为 虚拟 控制 量 , 即 zz =£ TCZ] —za Jill 
V, =—ciz 十 zizs 
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如 果 2, —0, WI] V0, 为 此 ,需要 进行 下 一 步 设计 。 
(2) 定义 Lyapunov PR 


La 
V 一 Vi 十 可 2 


由 于 zz 一 大 (zt) 十 g(zt)u 十 cizi 一 zi， 则 
V, =V, z.£;— —c0,z1 Hzizs: +z (f(x,t)+gla,t)u+cizi ~za) 
Afi V <0, Wit 


一 HE — caza — z1) in tanh (Eem | (12.2) 
其 中 ,cs me 为 正常 数 , 且 可 设 定 。 
事件 驱动 策略 为 : 
u =w(ti), Vt € [tstin) (12.3) 
taa —inf(t € R, |e) |2 m), t —0 (12.4) 


其 中 ,0 二 m m eG)-—.G)—u(G), 

针对 事件 驱动 策略 式 (12. 3) 和 式 (12.4), 可 分 析 如 下 : 当 zE[Liiyti41) 时 ,u 二 w(ti)， 
其 中 ,t=z,,) WEA lea) >m 确定 ; Ce GO <m 时 ,不 传输 控制 输入 信号 ,控制 器 的 值 
保持 不 变 。 

综 上 所 述 , 存 在 一 个 连续 时 变 系数 A(t) E A G0 m0,A G4) X1, BlAGO | fili 
w(t)=u(t) 十 A(t)m, 即 

u(t) =w(t)—à(t)m (12. 5) 

式 (12.5) 包 括 以 下 3 种 情况 : 

(1) MAGO-—0HBf.e()—oG)—u(G)-—0,J B] t=t, u—QG,). 

(2) 34lAGO | «1Ríf.leCGOl = le Go) ut) m, IBI tE [t, t4. 

G) 3]jAGO| —1 Bb. |e) | m ,确定 当前 时 刻 为 :==ti41 GRE uo Gu. 

NF t€ [ta t2 JA EE ISI) E. 因此 ,对 于 任意 时 刻 , 都 满足 m (22 —u Q0 HAm, 
其 中 ,A(z) 满 足 A(t)==0,X4(t441)== 土 1, 且 14(1)| 过 1。 


则 
V, ——c, zi ziz: xf rst) dg GGG) — Alm) c£; —] 
—-—c,zi—c,z — zag (x t tanh (EPER) gpg (DA Gm 
«C— c,zi—c,z2; — gg m ‘mtanh (E522) +| zig (x,t) |m 
由 于 站 
dsl egien jA -zag (zmtanh (EPE) a o, 2785€ 
DU 


V, €— cz! — crz? 4- 0. 2785e 
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即 
V， = V; +d 
其 中 ,17 一 2max(clycz),d 一 0.2785e 。 


从 而 得 到 指数 收敛 的 形式 


VG) € V,CDe" i 


HT V, d 4 t ， 则 zii, ETU CENCE AE 


Xy "X3 i 


12.1.3 时 间 驱 动 有 限 次 触发 分 析 


由 于 事件 驱动 不 能 无 限 次 触发 ,因此 需要 进行 有 限 次 触发 进行 分 析 证 明 。 通 过 以 下 两 
点 进行 分 析 ， 

CD 由 于 对 VtELt step) sum wlt) W eG)-—wQG)—uQG)-—oÀG)—0GsoG, 7) 为 
常数 , 则 eo), PCS Le | — sign (e) —signte)o ló |f V LC [zs ns HERE. 


由 于 g(x,t) 和 f(x,t) 和 其 一 阶 导数 连续 且 xz。 连续 ,因此 ww 连续。 又 因为 上 是 z， Mz 
的 函数 , 且 z 和 <, 有 界 , 因 此 一 定 存 在 一 个 正常 数 k 使 |w | 三 k。 
(2) C t€ [1, sti) ff Suo G0. HP ,t =t, WEH lelt) >m 确定 , 则 el(zi)==0 
lim leG21— leG,)l dim leQ)»| 


H lim le) 2m ia z lels lo | <k 可 得 二 一 -— T [k Bul 
LT Ék Let 04 


k Cri 7522: lim le) | >m, Bl iui nE 于 则 一 定 存在 正常 数 :" 0 na at, 


t S ER RT VL e JC RE f B0 Zeno behavior 3 $& ^ , 即 相 邻 两 次 触发 时 间 间 隔 之 
间 存 在 一 个 下 界 , 从 而 在 有 限时 间 内 不 能 无 限 次 触发 。 


12.1.4 仿真 实例 


被 控 对 象 取 式 (12.1), 取 f£ (rt) —25x,.g Gr, t)—133, W z,—sin(2xt2, Ht 6—10, 
m —0.50,m —1.0,c, —35.c, —35. T2 HERR (12. 32 — 3X (12. 40 ,仿真 结果 如 图 12-1 和 
图 12-2 所 示 。 


ideal position signal 


Position tracking | 


0 5 10 15 20 25 30 
time(s) 
(a) Ax E 


12-1 状态 跟踪 
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ideal speed signal 
speed tracking 


dyd.x2 


0 5 10 15 20 25 
time(s) 
(b) 状态 跟踪 


图 12-1 ( 续 ) 


> 
0 5 10 15 20 25 
time(s) 
图 12-2 控制 输入 
仿真 程序 如 下 : 


(1) 主 程序 ; chapl2_lmain. m, 


% Discrete controller for continuous plant 
clear all; 
close all; 


ts-0.001; % Sampling time 
xk = zeros(2,1); 

ul-0; 

for k -» 1:1:2000 

time(k) = k*ts; 


zd(k) = sin(2* pi*k*ts); 
dzd(k) = 2 * pi * cos(2 * pi * k * ts); 
ddzd(k) = - (2 * pi)^2 * sin(2* pix* kx ts); 


para=u_1; 

tSpan = [0 ts]; 

[tt, xx] = ode45('chap12_1plant', tSpan, xk, [ ], para); 
xk = xx(length(xx), :); 

xl(k) = xk(1); 


30 


30 
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x2(k) = xk(2); 


zl(k)- x1(k)- zd(k); 
多 dzl(k) = (z1(k) - zl 1)/ts; 


fx(k) = -25 * x2(k); 
gx(k) = 133; 
c1 = 35;c2= 35; 


zl(k) = xl(k)- zd(k); 
z2(k) =x2(k) * c1 * z1(k) - dzd(k); 
dzl(k) = x2(k) - dzd(X) ; 


epc = 10; 

m=0.5;mb= 1.0; 

w(k) = 1/gx(k) * (-fx(k) 一 clxdzl(k) * ddzd(k) - c2 * z2(k) - z1(k)) - mb * tanh(gx(k) * z2(k) 
* mb/epc) ; 


e(k) "w(k)-u 1; 
if abs(e(k))» 7 m 
u(k) = w(k); 
ul-w(k); 
else 
u(k) =u 1; 
end 


end 

figure(1); 

subplot(211); 

plot(time,zd, 'r',time,xl, 'k:', 'linewidth',2); 
xlabel('time(s)');ylabel('zd,x1'); 

legend( 'Ideal position signal', "Position tracking'); 
subplot(212); 

plot( time, dzd, 'r', time, x2, 'k: ', 'linewidth',2); 
xlabel( 'time(s)');ylabel( 'dyd, x2'); 

legend('ideal speed signal', 'speed tracking'); 


figure(2); 
plot(time,u, 'r', linewidth',2); 
xlabel('time(s)');ylabel('control input'); 


(2) 被 控 对 象 程序 : chapl2 lplant. m, 
function dx = PlantModel(t, x, flag, para) 
dx = zeros(2,1); 


u = para; 


dx(1) =x(2); 
dx(2)» -25*x(2) +133 * u; 


12.2 Æ Fe ENA 87 3E R zc AL 8 8] 


在 常规 方法 中 ,用 于 事件 驱动 的 国 值 m 为 固定 值 ,而 在 实际 控制 中 , 当 控制 输入 信和 号 值 
较 大 时 ,为 了 节省 通信 和 量 , 此 时 应 该 采用 较 大 的 m 值 , 当 控 制 输入 信号 值 趋 近 于 零 时 ,为 了 
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提高 控制 精度 ,此 时 应 该 采用 较 小 的 m 值 ,因而 需要 阅 值 m 为 时 变 的 。 


12.2.1 基本 原理 
被 控 对 象 为 


£i —3X 3 
(12. 6) 
r;— f (at) + g(r Eu 


KB gG 220. 
定义 Ti 为 角度 ,zs 为 角速度 ,定义 角度 误差 之 ] 二 zi 一 Za; 其 中 xg 为 指令 信和 号, 则 


£j], d= 


控制 目标 是 t>o}, r; >z dq Eus 
12.2.2 控制 器 的 设计 与 分 析 


反 演 控制 方法 设计 步骤 为 : 
(1) 定义 Lyapunov PR 
Vi 一 了 
则 
V, =z Z; =Z; (T —Za) 

取 xz; 三 一 cizli 十 zy 十 zx, 其 中 ,ci 二 0,z， 为 虚拟 控制 量 , 即 z =r +Hcizi za M 

V, =— cizi 十 iz， 
如 果 z, 二 0, 则 过 0。 为 此 ,需要 进行 下 一 步 设计 。 
(2) 定义 Lyapunov 函数 


1 
Vy =V, U 


由 于 z;,— f. gG Duc z,—2z,;. M 
V, 2V, -- z,2; =c? ert ea f r0 + gru cuz, —2,) 
为 使 V, 志 0, 设 计 


o =— i vtan (Se) -m1 +o) tanh (£z) (12, 7) 
其 中 ,mi ve AIEO 1,v——f (x.t) —c;z;-z,—0;2; 2150,70, 
3k T HE 2E [38] (EE 95) 3E (AF 3C] RE 2 
u-—woG., MEE Ld) (12. 8) 
tin —^inf(t € R, | et) | 之 6 | u |+mi} (12. 9) 


XB m 0m ge) —90)—uQ), 
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由 式 (12.9) 可 知 : 存在 一 个 连续 时 变 系 数 A; GO, WE A; (4,2 —0,A2 (4,44) — El, H 


PHONES E 
et) =u (t) +À, (t) | u |] H-m4) 


=u (t) +À, (t)ðu(tYsgnlu(t)) +A: Cm 
=(1 +å; (t)sgnlu(t)) ut) +å: Cm, 
=(1 FA; Sult) + A COm, 


其 中 , At) =à; (t)sgnlu(t)), FJa (1)| 志 1 Hlsgntéu GO)D| &i1,9|a, a) |= 


[As (@)sgnlu(t))|<S1. 
Bp 
eQt)-—(1-cA,;Q)DuG)--àAOom,. Nt € [t,.t,,) 
其 中 ,| ,GD)| 委 1,1A:()| 委 1。 


则 
qy 8992 — hi em 
MT T Id VUE 
j G2) As Gm 
V; =— 1 z E * -€— — BN - 
2 €1zj zits t (ro t)-- gir EE ENNOT g(a £3 ADS 
由 于 一 atanh (所)<0, 则 zo c so AT 
w(t) w(t) 
"TA ?1 
由 于 [Xs Dmi| 志 mi .1—8s 12-2, 29] B] 
| As GOm, < m, 
] +à, 26 一 这 
考虑 fxzst)--cizi—z4 ——9g—65£;— z; W 
: rCr.)z,m 
Vs 去 一 azi tziz n (fos Ege 7 eB tan za) + 
P "- y E )z,m è 
= clz!? + ziz: 十 zs(f (zx,t) —vtanh (S22) — g (æ i tanh (EE) 十 Cizi 一 之 
E E 


gG zm, 
1—26 


= —cziczimdz;)Cv9o—cm— z1) — zavtanh (^? :)- gG .t)z,m, tanh( 
E E 


gG zm, 


1—8 


ZU Z r(xt)z;m 
AZ— cizi — czs 十 | zv | z; vianb (727) —gGrsDssm tanb( S5) 十 
E € 


| gGeDzjm; | 


由 于 


| z;v |- z; vtanh (4? "< 0. 2785e 
€ 


Feizi =A 


g(x,t)z:m; 
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paratie i 2785e 
一 | & 0. 


0 x | z,gG. [my — zag (æ t)i tanh( 


则 
V, XC— cizi — cz; 4- 0. 557€ 
即 
V, <— qV: +d 
KB 9 —2max(c, 0?) d —0.557e, 


从 而 得 到 指数 收敛 的 形式 


VG) VC(0)e™ + 


由 于 Vs 一 让 二 十 二 =， Wee 有 界 且 收 全 到 2 全 ,如果 2 全 足够 小, 当 4oo 时 ， 


TI Zast” Zdo 


本 节 算 法 的 时 间 驱 动 有 限 次 触发 分 析 方法 可 参考 12. 1. 3 节 的 分 析 过 程 。 
12.2.3 仿真 实例 
被 控 对 象 为 单 力 臂 机 械 手 , 即 

5 一 一 了 (入 十 mglcosg) + T 


1 1 


取 rz,=0,zry 一 0 , 则 fG.t)—-— I (dx,--mglcosz, ),g Ge — T BUTLBIUS B ct 


m — 1 ,机 械 臂 长 度 /=0.25,d 三 2.0, 则 转动 惯量 T 一 子 ml。 


取 xj 一 Sin(2rt), 取 € — 10.m, =], 0,mi 一 2. 0.c; —35,c,— 35,12 dil HX C12. Ty 
式 (12. 9) ,仿真 结果 如 图 12-3 和 图 12-4 MR. 


ideal position signal | 


time(s) 


=~ ideal speed signal 
speed tracking 


time(s) 
12-3 ”状态 跟踪 
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control input 


0 0.5 ] 1:5 2 
time(s) 


12-4 控制 输入 


仿真 程序 如 下 : 
CD 主 程序 : chap12_2main. m, 


* Discrete controller for continuous plant 
clear all; 
close all; 


ts-0.001; % Sampling time 
xk = zeros(2,1); 

ul-0; 

fork-21:1:2000 

time(k) = k*ts; 


zd(k) = sin(2* pi*k*ts); 
dzd(k) = 2 * pi * cos(2 * pi * k * ts); 
ddzd(k) = — (2 * pi)^2* sin(2* pi * k ts); 


para=u_1; 

tSpan = [0 ts]; 

[tt, xx] = ode45('chap12_2plant', tSpan, xk, [ ], para) ; 
xk = xx(length(xx),:); 

xl(k) = xk(1); 

x2(k) = xk(2); 


zl(k) =xl(k) zd(k); 

%dz1(k) = (z1(k)-z1 1)/ts; 

g=9.8; 

m=1;l=0.25;d=2.0;I=4/3*m* 1^2; 

fx(k) = -1/I*(d*x2(k) * m*g*1*cos(xl(k))); 
gx(k) = 1/1; 
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c1 = 35;c2 = 35; 


zl(k) =xl(k) - zd(k); 
z2(k) =x2(k) * c1 * z1(k) - dzd(k); 
dzl(k) = x2(k) - dzd(k); 


epc = 10; 

delta - 0.20; 

ml = 0.15;m1b= 0.20; 

v(k) 7» - fx(k) - c1 * dz1(k) * ddzd(k) - c2 * z2(k) - z1(k); 

w(k) = - (1* delta)/gx(k) * v(k) * tanh(z2(k) * v(k)/epc) - mlb» (1 * delta) * tanh(gx(k) * z2 
(k) * n1b/epc); 


e(k) w(k)-u 1; 

if abs(e(k))» = delta * abs(u 1) * m1 
u(k) = w(k); 
ul-w(k); 

else 
u(k)7u 1; 

end 


end 

figure(1); 

subplot(211); 

plot(time, zd, 'r', time, x1, 'k: ', 'linewidth',2); 
xlabel('time(s)');ylabel('zd,xl'); 

legend('ideal position signal', 'position tracking'); 
subplot(212); 

plot(time, dzd, 'r', time, x2, 'k:', 'linewidth',2); 
xlabel('time(s)');ylabel('dyd,x2'); 

legend('ideal speed signal', 'speed tracking'); 


figure(2); 
plot(time,u, 'r', 'linewidth',2); 
xlabel('time(s)');ylabel('control input'); 


(2) 被 控 对 象 程序 : chapl2 2plant. m, 


function dx = PlantModel(t, x, flag, para) 
dx = zeros(2,1); 


u = para; 


xl =x(1);x2=x(2); 

g=9.8; 
m=1;1l=0.25;d=2.0;I=4/3%m* 1^2; 
fx- —-1/I* (d* x2*m*g*1*cos(x1)); 
gx = 1/I; 


dx(1) = x(2); 
dx(2) = fx + gx * u; 
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CHAPTER 13 


13.1 带 有 输入 延迟 的 输入 受 限 控制 


13.1.1 系统 描述 


考虑 具有 控制 输入 延迟 的 模型 为 
q +aq +bq =ul(t—r) 
其 中 ,rz 二 0 APERE M: 
控制 目标 为 : 4 toht, qg 0, q0. 


13.1.2 控制 器 的 设计 与 分 析 


定义 辅助 变量 为 
pg 
" =f u(0)dó 
则 
m Rai mE 
——u(t —:) —(1—a)4q-- bq —u - u(t — 7) 
——(1—a)4--bq—u 
设计 控制 律 为 
u —ktanhr 
其 中 ,之 0。 
将 式 (13.4) 代 入 式 (13. 3) ,可 得 


r——(1—a)q-4-b5q — ktanhr 
设计 Lyapunov KO 


1 2 . ? 
V-—Xmr x 2 hi4 十 卫 


其 中 , P =o| (['u*cooa0) às y. 2 0,2, 2» 0, 2» 0, 
根据 引 理 13. 1( 见 本 章 附 录 ) ,对 己 求 导 , 得 
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(13. 1) 


(13. 2) 


(13. 3) 


(13. 4) 


(13. 5) 
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P on! —of u? (0) dà (13.6) 
由 于 双 曲 正切 函数 满足 |tanhzx | 壹 |x| 中 ,由 于 xtanh(x)==x 5 Tm 0, WA 
eTe 
0 x tanh'x < rtanhr (13. 7) 


对 V 求 导 可 得 
V —q5 r£ + pqg +P — 4, r C7 (0 — a24 +bq — ktanhr) + q4 +P 
<— (0— a)giQr 4- bg qr — kmtanh’r + qd + wrk’ tanh?r 一 | «cae 
— — (km — wrk? )tanh'r — (1 —a)m(C—4 —q —e,) + 
byia C4 —4 — 6, 4- quad -of uD 


=— (kn — wrk’ )tanh’r + (1 —a)qg? FHI 一 a)m 一 bm d 9:209. 


(1 — ame, — ma^ bye, —o| u? (0)dà (13.8) 
根据 Cauchy-Schwartz 不 等 式 
"E u? (0)dà (13.9) 
则 
=w u* (40 «— ^e: (13. 10) 
将 式 (13. 100 AR C13, 8) ,可 得 
V «— (eg, — wck!)tanh! r + (10 — apg? -- Cd —a)g, — ony) dg 
Q — aq de, — bp q* — bye; — Se? 
上 式 可 写 为 二 次 型 的 形式 
vv 去 BTWB 
其 中 ,8 —[tanhr à Q &li 
| — (kn — wrk’) 0 0 0 
0 — bn E —a)y, — bn t 9?) 一 王妃 
W= 1 1 
0 3 (17a — bp +y) (1—a)3, ;0-o05 
1 il w 
b 0 gon Fn = | 
(13.10) 


n AE SEXE CR omi gs 和 w 使 W=0, 即 W 特征 值 均 为 负 , 则 W 负 定 , 则 


V0 


由 于 V 写 0, 根 据 V 过 0, 则 V 有 界 。 根 据 式 V 和 prW8 , 当 V=0 时 ,gq 二 0,9 =0, 根 据 
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LaSalle 不 变 引 理 , 当 :一 ce 时 ,g 一 0,g 一 0。 且 由 双 曲 正切 函数 性 质 可 知 |x| 委 |&||tanhr| 委 4。 
Fmincon 是 用 于 求解 非 线 性 多 元 函数 最 小 值 的 MATLAB 函数 ,优化 工具 箱 提供 

Fmincon 函数 用 于 对 有 约束 优化 问题 进行 求解 。 针 对 本 问题 ,在 参数 满足 一 定 约 束 范围 条 

件 , 且 W 特征 值 均 为 负 的 条 件 下 ,采用 Fmincon 函数 求解 式 (13. 1D PS k py Mw, 


13.1.3 仿真 实例 


针对 模型 式 (13. D.C a —2, 5b 2.06. kg n, 和 w 均 满 足 约束 [0 10], 采 用 
Fmincon 函数 求解 式 (13. 11) ,Fmincon 函数 按 满足 参数 约束 范围 及 W 为 负 定 来 进行 优化 
求解 ,可 得 &=0. 0335, 此 时 W 的 特征 值得 最 大 值 为 一 0. 03, 满 足 W 负 定 。 采 用 控制 律 
式 (13. 4) ,仿真 结果 如 图 13-1 和 图 13-2 所 示 。 


0 10 20 30 40 50 60 
time(s) 
1 
0 
NR 
& 
$ -2 
E 
-3n 
-4 
0 10 20 30 40 50 60 
time(s) 


图 13-1 位 置 和 速度 响应 


e 
e 
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b 
e 
N 


control input at t 
So 


S 


10 20 30 40 50 
time(s) 


o 
已 


control input with delay 
o 


$ g 
3 


”0 10 20 30 40 50 
time(s) 


图 13-2 控制 输入 
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仿真 实例 如 下 : 
1. Fmincon 求解 程序 
CD 主 程序 : chapl3_lmain. m, 


clear all; 

close all; 

多 初始 值 

xg s [1.1,2,;15 

& 四 个 参数 xitel, xite2, w, k 的 范围 ,分 别 为 最 小 值 和 最 大 值 

x min = [0000]; 

x max - [10 10 10 10]; 

kk = fmincon((Q(x)chap13 1func(x),x0,[],[], [], [ ], x min, x max); 
chapl3 1func(kk)  * 最 大 特征 值 

save delay filel kk; 


(2) 子 程序 : chapl3 lfunc. m, 


function eig max - max eig func(x); 


a = 2; 

b = 2; 

tau = 6; 
xitel = x(1); 
xite2 = x(2); 
w 7 x(3); 

k = x(4); 


W1 = [-(k*xitel-w*tau*k^2) 000]; 

W2 = [0 -b*xitel 0.5*((1—-a) * xitel- b* xitel t xite2) -0.5*b* xitel]; 

W3 = [00.5*((1-a) * xitel- b* xitel + xite2) (1-a) * xitel 0.5 * (1-a) * xitel]; 
W4 = [0 —0.5*b* xitel 0.5* (1-a) * xitel - w/tau]; 

W = [W1;W2;W3; W4]; 

eig_value= eig(W); 

eig_max = max(real(eig value)); 


end 


2. Simulink 仿真 程序 
(1) 控制 系统 Simulink 主 程序 : chapl3_1sim. mdl。 


messo! Transport 
Delay1 
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(2) 控制 器 S 子 数 程序 ; chap13_1lctrl, m, 


function [sys,xO,str,ts] = spacemodel(t,x,u, flag) 
switch flag, 
case 0, 
[sys, x0, str, ts] = ndlInitializeSizes; 
case 1, 
sys = ndlDerivatives(t,x,u); 
case 3, 
sys = mdlOutputs(t, x, u); 
case {2,4,9} 
sys=[]; 
otherwise 
error(['Unhandled flag = ',num2str(flag)]); 
end 
function [ sys, x0, str, ts] = mdlInitializeSizes 


sizes = simsizes; 


sizes. NumContStates 


sizes. NumDiscStates = 


sizes. NumInputs = 


0 
0 
sizes.NumOutputs = 1; 
3 
sizes.DirFeedthrough = 1 
1 


sizes.NumSampleTimes 
SyS = simsizes(sizes); 
x0 []; 

str IB 

ts - [00]; 


function sys = mdlOutputs(t, x, u) 
persistent kk 
if t== 

load delay filel; 


end 
th=u(1);dth= u(2); 
ez = u(3); 

r= -dth- th- ez; 
k= kk(4); 


ut=kx* tanh(r); 
sys(1) = ut; 


(3) 被 控 对 象 S 郴 数 程序 : chapl3 lplant. m, 


function [sys,x0, str,ts] = s function(t, x,u, flag) 
switch flag, 
case 0, 
[sys, x0, str, ts] = ndlInitializeSizes; 
case 1, 
sys = ndlDerivatives(t,x,u); 


case 3, 
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sys = mdlOutputs(t, x,u); 
case (2, 4, 9 ) 
sys = []; 
otherwise ， 
error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0, str, ts] = mdlInitializeSizes 


Sizes - simsizes; 


sizes.NumContStates 


sizes.NumDiscStates 


sizes.NumOutputs 


sizes.NumInputs 


sizes.DirFeedthrough 


LU 
oOo O e N O N 
A 


sizes. NumSampleTimes 
Sys = simsizes(sizes); 
x0 -[5.00]; 

str -[]; 

ts -[] 

function sys = mdlDerivatives(t, x,u) 
a= 2;b= 2; 

ut 7» u(1); 


sys(1) = x(2); 
sys(2) -ut- a* x(2) - b* x(1); 
function sys = mdlOutputs(t, x, u) 
sys(1) = x(1); 
sys(2) = x(2); 


(4) 作 图 程序 : chapl3 l1plot. m, 


close all; 


figure(1); 

subplot(211); 

plot(t, y(:,1), 'k', 'linewidth',2); 
xlabel('time(s)');ylabel('q1 response'); 
subplot(212); 

plot(t, y(:,2), 'k', 'linewidth',2); 
xlabel('time(s)');ylabel('q2 response"); 


figure(2); 

subplot(211); 

plot(t,ut(:,1), 'r', 'linewidth',2); 
xlabel('time(s)');ylabel('control input at t'); 
subplot(212); 

plot(t,ut(:,2), 'r', 'linewidth',2); 
xlabel('time(s)');ylabel('control input with delay'); 
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13.2 基于 干扰 观测 器 且 带 有 输入 延迟 的 输入 受 限 控制 


13.2.1 系统 描述 
考虑 带 扰动 的 具有 控制 输入 延迟 的 模型 
q d- aq +bq —u(t —:) d (13. 12) 
KB. 时 间 延 迟 常数 ,d 为 扰动 ,|d| 委 D。 
控制 目标 为 : 4 tokt, 0, q0., 


假设 1: 扰动 是 慢 时 变 的 ,假设 4 ==0。 
13.2.2 控制 器 的 设计 与 分 析 


定义 辅助 变量 如 下 . 
r=—k,(q+e.)—k:q 
i (13. 13) 
i -| u(g)db 
其 中 ,v (90)==ktanhr。 
设计 扰动 观测 器 为 : 
=K(ad+ba—utt—r))— Kd 
(13. 14) 
d —z + Kq 
其 中 ,qd 为 d 的 估计 值 , 且 开 之 0。 
gg X d—d —d , 则 
de-0 a —N Kt gut 0) EE E 
——K (q -- ad --bq — u(t —7)) d- Kd 
——Kld-4ye—Rd 
设计 
Ta 
Yu 一 7d 
则 
V, ——Kd* 去 一 2KV， 
因此 干扰 观测 器 指数 收敛 , 即 :一 oo 时 ,d 一 0 且 指 数 收敛 。 
设计 控制 律 为 
ü(1) - v) —d) (13. 15) 
Kp.a0, 
则 


r=—ki(g el) — kq 
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——h, C^ ad —bq -- u(t — £) -d -v(1) —v( — £0) — hd 

——hk, C^ ad —bq d- v(t —£) —d(t — r) +d) +o) — v(t —r))— kd 
——hk, C^ ad — bq 4-v(1) —d (t — £) d (4) —d lt) 4-4 (1) — kag 
=—k,(—aġ—bq +v) —d G — e) +4) H3 (1) — kd 


— (ak, =k) t og — k,ktanhr — kid (1) — gh (d (4) —d (t — c) 
设计 Lyapunov 函数 为 


V—Lnr bU EP qd (13. 16) 


其 中 , P=o| (f'o 0d) dsp > 0: 09 > 0w 7 0. 
根据 引 理 13. 1( 见 本 章 附 录 ), 对 P 求 导 得 


Pen! of vanae (18.17) 
DU 
V=mrr 十 72d9 +P dd 
—qr[XGk, —k:)ġ +bkiq —k,ktanhr — kid (t) — k, (d0) —dG —1))] + 
7:qd +wro’ —w| vað — Kyd’ 
«— gk, ktanhr + mr Caki — i24 +bkiq — kid) — qkir(d 0 —d (t — 0) + 
Mag + erv' -oj v00 — Ky, d* 
— — yk, ktanbr +q, C— ki (q +e.) — ksq) (Cab, — k;0q o bkiq — kid) + 
q:qd + wro’ -wf vi(0)d0— Kyd’ —mkir(d (t) —d (t —r)) 
其 中 ， 


m (一 Ai q Te) — ksq0)CCak, —k:)q + bkiq — kid) 
— —nk, (ak, —k,)g’ — [ys (ak, =k;) 二 mibki laqg — yık, Cak, —k2)ge. 十 
mk?ad — mbkiksq” Ex mbkiqe. T jk hs qd ct pied 
根据 Cauchy-Schwartz 不 等 式 


esr| 


Li 


v? (0) dó (13. 18) 
则 
-«| vè Q8 <— “e? 
V «— qikıktanh’r — qik, Cab, — kd! — [3s Cab, — k) + Mbk? aa 一 
ıkı Cak, — k: )ge。 2 mkigd — mbkk.q* mbkige. + mkiksqd + mkie.d d 


1299 + wro’? -vf v' (0)d0 — Kyd? — gkir(d 1) 一 dt 一 r)) 


Re MM M 


<— pk, ktanh'r — yk Xak, — k;2d* — [gi ks Cab, — k3) + gibki lag 一 
miei Cak, — kioge. + y bidd — mbkiksg’ — y bkiqe. + mkikgd d- gi kie.d + 
0:qd + orv? a — Kyd’ — gk,r(d 1) —d (t — 20) 
由 于 wro —wrk^ tanh' r , Bil 
V «— gb, ktanh!r — y Ei Cab, — k)? — [ks Cab, — k) + mbk? aa 一 
qmi Cab, —k:)qe. + m kiqd 一 TiORIR29 — qbkige. + p kik;qd -- gy kie.d + 
q:4d + ck tanh’ r — Se? — Kg,d! — qikır(d (t) —d (t — t) ) 
= (wrk? — qikik)tanh’r — gk, Cak, — k )ġ? 一 
[mk (ak — kz) + mbk? +q:]qq — qıkı Caki — k:)qe. + pk? gd 一 
mokiksg’ — q bkiqe, +mkikagd — e: T qhie.d — Kg,d! 一 
mkir(d(t) —d G — 0») 
则 
V « Bg'WB — y, kir(d (t) 一 dt 一 r)) 


B — [tanh > d q e, d] 


[orR — mkik 0 0 0 0 

0 — mk Gk, — ka)  — [gk Gk, —k)--gbki — p] -mki laki— k) nk 

w= 0 0 — p bk, kh; — n bki p kk. 
0 0 0 m m 

| 0 0 0 0 — Ky. 
(13. 19) 


若 存在 quoq 0970, K 0, b, hk 使 得 W 负 定 , 则 可 实现 有 界 收敛 。 由 于 
1 一 co 时 ,4d 一 0 上 且 指数 收敛 ,如 果 扰 动 为 常 值 或 慢 时 变 , 则 a — d (1 — 0) 0. fi d (7) 一 
dd 一 rz) 一 0, 则 
V < B'WB — y kir(d i) —dG — 0) —8'WB < 0 
HF Vco.dRdE Vo. V 有 界 。 RIE VAW 二 0, 如 果 W 负 定 , 当 V 夺 0 时 ,p = 
0, 根 据 LaSalle RÆ, ^4 ;—ooHf.B —[tanhr q q e. d] —0.H xi iE 9) eg 


性 质 可 知 |u| 志 |k||tanhr| 十 |4 I, EE -D-- ld Cool. 
针对 本 问题 ,在 参数 满足 一 定 约束 范围 条 件 ,采用 Fmincon 函数 进行 优化 求解 , 取 W 的 
最 大 特征 值 作为 优化 指标 ,从 而 得 到 满足 W 特征 值 均 为 负 的 Fy ys wsK ki Rs。 


13.2.3 仿真 实例 
针对 模型 式 (13. 122. a —2. b 2.7 6.d =3+0. 3sin(0. 122, SE TE XE 2891 (E I 
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[1 0], 观 测 器 式 (13. 14) 中 ,初始 值 取 4(0) B Mw 和 KK 约束 取 [0 10]， 
kik, 和 上 约束 取 [0 1], 采 用 Fmincon 函数 按 满足 参数 约束 范围 及 W 为 负 定 来 进行 优化 
求解 ,可 得 [yy nw K kiska k]=[9.4103 5.0000 5.0000 4.4254 5.0000 
0.9410 0.5000 ”0.3441], 从 而 可 得 ==0.3441, 上 |u| 二 3 十 3. 3—6. 3。 

此 时 W 特征 值 的 最 大 值 为 一 1. 4751, 满 足 W 负 定 。 采 用 控制 律 式 (13. 15) ,仿真 结果 
如 图 13-3 一 图 13-5 所 示 。 


ql response 
一 N w 
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图 13-3 位置 和 速度 响应 


control input at t 
o 
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图 13-4 控制 输入 
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>: 
0 20 40 60 80 100 


time(s) 


input disturbance 
N 
S 


S 


0 20 40 60 80 100 
time(s) 


Æ 13-5 e, 及 扰动 的 变化 


仿真 实例 如 下 。 
1. Fmincon 求解 程序 
d) 主 程序 : chap13_2main. m, 


clear all; 
close all; 


cv 
LU LU 
Il e e 


tau 
名 初始 值 xitel, xite2, xite3, w, K, k1, k2, k 

k0 = [00000000]; 

& 三 个 参数 的 范围 ,分 别 的 最 小 值 和 最 大 值 

k min = [00000000]; 

k max = [10 10 10 10 10 1 1 1]; 

p = [a,b, tau]; 

kk = fmincon(@(k)chapl3 2func(p,k),k0,[],[],[],[],k min,k max) 
chapl3 2func(p,kk) 

save delay file2 kk; 


(2) 子 程序 : chapl3 2func. m, 


function eig max - max eig new(pl,x); 


a = pl(1); 
b = p1(2); 
tau = p1(3); 


xitel = x(1); 
xite2 = x(2); 
xite3 = x(3); 
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x(4); % k4 
K = x(5); % k5 


z 
LU 


kl = x(6); 
k2 = x(7); 
k = x(8); 


Wl = [w*tau* k^2- xite1* k1*k 0000]; 

W2 = [0 - xitel * k1 * (a*kl-k2) - xitel * k2 * (a* k1- k2) - xitel * bx k1^2 + xite2 - xitel 
*kl* (axkl—k2) xitel x k1^2]; 

W3 = [00 -xitel * b* k1* k2 -xitel * b * k1^2 xitel x kl * k2]; 

W4 = [0 0 0 —w/tau xitel * k1^2]; 

W5 [0000 -K»* xite3]; 

W = [W1;W2;W3;W4;W5]; 


eig value = eig(W); 
eig max = max(real(eig value)); $% optimum objective 


end 


2. 控制 系统 仿真 程序 
(1) Simulink 主 程序 : chap13_2sim. mdl。 


(2) 控制 器 S 函数 程序 : chapl3 2ctrl. m, 


function [sys,x0, str,ts] = spacemodel(t, x, u, flag) 
switch flag, 
case 0, 
[sys, x0, str, ts] = mdlInitializeSizes; 
case 1, 
sys = ndlDerivatives(t,x,u); 
case 3, 
sys = mdlOutputs(t, x,u); 
case {2,4,9} 
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sys-[]; 
otherwise 

error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0, str, ts] = ndlInitializeSizes 


Sizes = simsizes; 


LU 


sizes.NumContStates 


sizes.NumDiscStates 
sizes.NumOutputs 


sizes.NumInputs 


I 


sizes.DirFeedthrough 


i 
.earoo 
Ara: 


sizes. NumSampleTimes 


sys = simsizes(sizes); 


x0 - [E 
str e LI; 
ts - [00]; 


function sys = mdlOutputs(t, x, u) 
persistent kk 


ez 7 u(1); 
q-u(2); 
dq= u(3); 
if t== 
load delay file2; 
end 
kl = kk(6); 
k2 = kk(7); 
k= kk(8); 


r= -kl*(dqtez)-k2*9g; 
namna = 0; 

vt=kxtanh(r) * namna * sign(r); 
% ut = vt- dp; 

sys(1) = vt; 


(3) 观测 器 S 函数 程序 : chapl3 2obv. m. 


function [sys,x0,str,ts] = s function(t, x,u, flag) 
switch flag, 
case 0, 
[ sys, x0, str, ts] = ndlInitializeSizes; 
case 1, 
sys = ndlDerivatives(t, x,u); 
case 3, 
sys = mndlOutputs(t,x,u); 
case (2, 4, 9 } 
sys = []; 
otherwise 
error(['Unhandled flag = ',num2str(flag)]); 
end 
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function [sys, x0, str, ts] = ndlInitializeSizes 


Sizes - simsizes; 


sizes.NumContStates 


sizes.NumDiscStates 


sizes.NumOutputs 
sizes.NumInputs 


L 
orn &-25oc0n2 


sizes.DirFeedthrough 


sizes.NumSampleTimes 


sys = sinsizes(sizes); 


x0 -[0]; 
str -[]; 
ts -[] 


function sys = mdlDerivatives(t,x,u) 
persistent kk 
ift-- 
load delay file2; 
end 
K= kk(5); 


a-1.2;b-721.0; 


u tol-u(1); 
q= u(2); 
dq= u(3); 
z= x(1); 


dp=z+Kx dg; 


sys(1)= Kx (a* dq* b*xq- u tol) - K * dp; 
function sys = mdlOutputs(t, x, u) 
persistent kk 
ift--0 
load delay file2; 
end 
K= kk(5); 
u tol-u(1); 
q-7u(2); 
dq 7 u(3); 
z-7x(1); 
dp= z + K* dq; 
sys(1) = dp; 


(4) 被 控 对 象 S 函数 程序 : chap13_2plant. m, 


function [sys,x0,str,ts]- s function(t, x,u, f lag) 
switch flag, 
case 0, 
[sys, x0, str, ts] = ndlInitializeSizes; 
case 1, 
sys = ndlDerivatives(t,x,u); 
case 3, 
sys = mdlOutputs(t,x,u); 
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case (2, 4, 9 ) 
sys = []; 
otherwise 
error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0,str,ts] = mdlInitializeSizes 


sizes = simsizes; 


sizes.NumContStates 
sizes.NumDiscStates  - 


2 
0 
sizes.NumOutputs = 3; 
sizes.NumInputs = 1 
sizes.DirFeedthrough = 0 
sizes. NumSampleTimes = 0 
sys = simsizes( sizes); 
x0 -[1.00]; 
str = []; 
ts -[] 
function sys = ndlDerivatives(t, x,u) 
as 1.2; 
b= 1.0; 
u tol-u(1); 


dt-3-*0.0*sin(0.1*t); 

sys(1) = x(2); 

sys(2) = -a* x(2) - b* x(1) *u tol * dt; 
function sys = mdlOutputs(t, x, u) 
dt-3-*0.0*sin(0.1*t); 

sys(1) = x(1); 

sys(2) = x(2); 

sys(3) = dt; 


(5) 作 图 程序 : chapl3. 2plot. m, 


close all; 


figure(1); 

subplot(211); 

plot(t, y(:,1), 'k', 'linewidth',2); 
xlabel('time(s)');ylabel('q1 response'); 
subplot(212); 

plot(t, y(:,2), 'k', 'linewidth',2); 
xlabel('time(s)');ylabel('q2 response'); 


figure(2); 

subplot(211); 

plot(t,ut(:,1), 'r', 'linewidth',2); 
xlabel('time(s)');ylabel('control input at ey; 
subplot(212); 

plot(t,ut(:,2), 'r', 'linewidth',2); 
xlabel('time(s)');ylabel('control input with delay'); 
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figure(3); 

subplot(211); 
plot(t,ez(:,1), 'r', 'linewidth',2); 
xlabel('time(s)');ylabel('ez'); 

subplot(212); : 

plot(t, y(:,3), 'r', t, dp( :, 1), 'b', 'linewidth',2); 
xlabel('time(s)');ylabel('input disturbance'); 


13.3 基于 状态 观测 器 的 输入 延迟 控制 


13.3.1 系统 描述 


考虑 具有 控制 输入 延迟 的 系统 如 下 : 
q +aq +bq —u(t — cv) (13, 20) 
Kp c0. c 为 延迟 时 间 常 数 。 
考虑 具有 控制 输入 延迟 的 系统 : 


q —Aq 二 Bu(i—r) 
(18.21) 


y —Cq 


0 1 r 
其 中 ,gq 二 [gi aT c0 为 时 间 延迟 党 数 ,4 一 | “， ]-a-to I]'6-[1 0]. 
a ^j 


a 


控制 目标 为 : 在 只 有 gi 为 可 测 时 o4 tht, q0. 
13.3.2 控制 器 的 设计 与 分 析 


为 了 实现 无 需 g; 的 控制 ,设计 状态 观测 器 为 : 
z —Az + Bu(t —:)d-LCy — z4) C13. 22) 
其 中 ;= 二 [zs za] L= 6]. 
4 e—q-—z Jj 
z —Az + Bu (t — r) + LCe 
e —Ae — LCe = (A — LC )e =A, e 


其 中 ,A =A- LC. 
按 负 定 设 计 Ai, 便 可 以 实现 ti 一 co 时 ,e 一 0, 即 E, >q Z2 "Qo. 


定义 辅助 变量 如 下 : 
r ——K' (z 十 Be.) (13.23) 
其 中 ,KK=[&; k]'.k0,:51,2. 
定义 
" -| u(0)dð (13. 24) 
设计 控制 律 为 
u(t) -—ktanhr (13, 25) 


其 中 ,之 0。 
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则 
r——K(z- Be.) 
— —K'[Az + Bu(t — :) 4- LCe + Bu(t) — Bu(t —2)] 
— —K'[Az 十 LCe + Bu (1)] 
— —K'[Az -- LCe + Bktanh(r)] 
设计 Lyapunov 函数 为 
Ve ye Red P (13. 26) 
其 中 , P -| (Fu cooao) ds, > 0,R > 0,0 77 0, 
根据 引 理 13. 1( 见 本 章 附 录 ) ,对 P 求 导 得 
P =ou’ —of u? (0)d0 (13. 27) 
对 V 求 导 , 可 得 
V=mi+e' Ré +P 


— — grK' (Az + LCe + Bktanhr) -- e" RA,e wru’ -«| u^ C0) dO 


= — K" Bkrtanhr ES mK (Az 十 LCe) -- e' RA,e + wru’ 一 of u^ (0)d0 


由 于 双 曲 正切 函数 满足 |tanhzx [cL LU ,ztanhz=x Se >o 
e e 


0 x tanh'r x rtanhr 


根据 Cauchy-Schwartz 不 等 式 , 有 


| u^ (0)d0 (13. 28) 

则 

-«| u? (0)d0 «— Se? 

t—r T 
V «— kK" Btanh'r — qrK" (Az 十 LCe) -- e" RA,e twrk’tanh’r — Se? 
€ 
= (wrk’ — kK" B)tanh’r — 9[ — K" (z + Be.) |K' (Az + LCe) - e' RA,e — TU 
= Cork! — ykK" B)tanh! r 十 7(z -- Be.) KK" (Az 4- LCe) 4- e' RA,e — Se? 
T 
= (wrk! — kK" B)tanh’r + zz KK Az + yz! KK LCe + 
qe .B' KK" Az 十 Me.BTKKTLCe + e" RA,e — Če? 
T 

由 于 


Te.BTKKTAz 一 jzTATKKTBe- 
.BTKKTLCe = ye" C'L'KK' Be, 
qe "e 
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V < Cork? — gkK" Botanh!r + gz' KK' Az 十 了 TIKKTILCe 十 


y. A" KK' Be. + ge' C'L'KK' Be. -- e'RA,e 一 一 


从 而 
V < g'wg 
B-—[tanhr z" el e,]' 
wrk” 一 qkK' B 0 0 0 
0 jKK'A zKK'LC  7A'KK'B 
We 0 0 RA,  xC'L'KK'B (13. 29) 
0 0 0 = 过 


E 


车 存在 >00 >0,K>0,R EE ,k>0 使 得 W 负 定 , 则 存在 «0. V< llel? 
根据 Lyapunov-Krasovski 稳定 性 定理 得 ,zx 和 e HAMARE, MARAE E q =z te 全 局 一 
致 渐进 稳定 ,zce 时 ,qg 一 0。 

由 W 的 表达 式 可 见 ,参数 mo LR K LR 的 选取 会 影响 系统 的 收敛 性 。 


13.3.3 仿真 实例 


针对 模型 式 (13. 20), 取 4a 二 1.2,6 二 0.1,r 二 3.0, 被 控 对 象 初 值 取 [1 0]。 状 态 观 测 器 
采用 式 (13. 22) ,初始 值 取 [0 0]. 

采用 Fmincon 函数 进行 优化 ,参数 [7 Ru Ri Ra Rav i hib; ] 的 约束 都 
取 [0 10], 采 用 Fmincon 函数 按 满 足 参 数 约 束 范围 及 六 为 负 定 来 进行 优化 求解 ,可 得 满足 
W 负 定 的 一 组 参数 为 : [y qo Ta \w\、K kik: k]=[1.1089 1.0266 1.0266 1.0266 
1.0266 0.8197 0.4601 0.4601 0.4601 0.5527 0.2261], 此 时 W 特征 值 的 最 大 值 
为 一 1.2527。 从 而 可 得 & —0. 2261 ,采用 控制 律 式 (13. 25), 则 |u | 0. 2261。 仿 真 结果 如 
图 12-6 和 图 13-7 所 示 。 

仿真 实例 如 下 。 

1. Fmincon 求解 程序 

(1) 主 程序 : chap13_3main. m, 


clear all; 
close all; 


= 1,23 
b-20.1; 
tau = 0.10; 
名 初始 值 xite, R11,R12,R21,R22,w,L1,L2,kl,k2,k 
x0 = zeros(11,1); 
s 三 个 参数 的 范围 ,分 别 的 最 小 值 和 最 大 值 
x min = zeros(11,1); 
x max = 10*ones(11,1); 
x max(end- 4:end) = ones(5,1); 
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ql and zl response 
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q2 and z2 response 
5 $ 


N 
e 


20 40 60 80 100 


time(s) 


图 13-6 ”状态 观测 及 状态 响应 
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control input with delay control input at t 
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time(s) 
图 13-7 控制 输入 


p = [a,b,tau]; 
kk = fmincon((Z2(x)chap13 3func(p,x),x0, [], [], []; [], x min, x max) 
chapi3 3func(p, kk) 


save delay file3 kk; 
(2) 子 程序 : chapl3, 3func. m, 


function eig max = max eig new(pl,x); 
a = pl(1); 
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b = p1(2); 
tau = p1(3); 


A= [01;-b =a]; 
B = [0;1]; 
C = [10]; 


xite = x(1); 

R = [x(2) x(3);x(4) x(5)]; 
w = x(6); 

L = [x(7);x(8)]; 
[x(9);x(10)]; 

k = x(11); 

AL = A-L*C; 

ei AL = eig(AL); 


Psi 
LU 


WO 2 2 = zeros(2,2); 
WO 2 1 = zeros(2,1); 
WO 1 2 = zeros(1,2); 


Wl = [w* tau* k'2- xite* k* K'*BWO0 1 2W0 1 20]; 

W2 = [WO 2 1 xite* K* K' A xite* K* K'KL*C xite* A' *K* K' * B]; 
W3 = [WO 2 14/0 2 2R* AL xite* C'*&L'* K* K' * B]; 

W4 = [0W0 1 2W0 1 2 -w/tau]; 


W = [W1;W2;W3;W4]; 


eig value = eig(W); 
eig max - max(real(eig value)); 


end 


2. 控制 系统 仿真 程序 
CD 控制 系统 Simulink 主 程序 : chapl3 3sim. mdl, 


chap13 3olw 
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(2) 控制 器 S 函数 程序 : chap13_3ctrl. m, 


function [sys,xO,str,ts] = spacemodel(t, x,u, flag) 
switch flag, 
case 0, 
[sys, x0, str, ts] = mdlInitializeSizes; 
case 1, 
sys = ndlDerivatives(t,x,u); 
case 3, 
sys = ndlOutputs(t,x,u); 
case (2,4,9) 
sys-[]; 
otherwise 
error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0, str, ts] = ndlInitializeSizes 


Sizes = simsizes; 


sizes.NumContStates 


sizes.NumDiscStates 


sizes. NumOutputs 


sizes.NumInputs 
sizes.DirFeedthrough 
sizes.NumSampleTimes 


Mu 
E^ WW OS 
€ 


sys = simsizes(sizes); 

x0 BF 

SEE — []; 

ts - [00]; 

function sys = mdlOutputs(t, x, u) 


1l 


persistent kk 
ez = u(1); 
z = [u(2);u(3)]; 


if t==0 
load delay_file; 
end 
B = [0;1]; 
K = [kk(9); kk(10)]; 
k = kk(11); 
r- -K'* (z + B*ez); 
ut = k* tanh(r); 
% ut = r; 
sys(1) = ut; 


(3) 观测 器 S 函数 程序 : chapl13 3obv..m., 


function [sys,x0, str,ts] =s_function(t, x,u, flag) 
switch flag, 
case 0, 


[sys, x0, str, ts] = ndlInitializeSizes; 
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case 1, 
sys = ndlDerivatives(t,x,u); 
case 3, 
sys = mdlOutputs(t, x,u); 
case (2, 4, 9 ) 
sys = [E 
otherwise 
error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0, str, ts] = mdlInitializeSizes 


sizes - simsizes; 


sizes.NumContStates 


sizes.NumDiscStates 


sizes.NumOutputs 


i 
O rPe UNON 
<s 


sizes.NumInputs 


sizes.DirFeedthrough 


sizes.NumSampleTimes 


sys = simsizes(sizes); 
x0 =[00]; 

str =[]; 

ts -[] 


function sys = mdlDerivatives(t, x, u) 
persistent kk 
ift -- 

load delay file; 


L = [kk(7);kk(8)]; 


a= 1.2; 
b = 0.1; 
A = [01;-b -a]; 


B. = T6071]; 
c= [10]; 


z= [x(1);x(2)); 
u tol = u(1); 
[u(2);u(3)]; 


ia 
外 


由 
上 


q- zi 


dz = A*z + Bxu tol + L*C*e; 
sys(1) - dz(1); 

sys(2) = dz(2); 

function sys = mdlOutputs(t, x, u) 


x(1); 
x(2); 


sys(1) 
sys(2) 


ERU ECL uUo noii od ne 


(4) 被 控 对 象 S 函数 程序 . chapl13 3plant. m, 


function [sys,x0,str,ts] = s function(t, x, u, f lag) 
switch flag, 
case 0, 
[sys, x0, str, ts] = mndlInitializeSizes; 
case 1, 
sys = ndlDerivatives(t,x,u); 
case 3, 
sys = ndlOutputs(t,x,u); 
case (2, 4, 9 } 
sys - []; 
otherwise 
error(['Unhandled flag = ',nun2str(flag)]); 
end 
function [sys, x0, str, ts] = mdlInitializeSizes 


Sizes = simsizes; 


OO OP NO Nx 


sizes.NumContStates 
sizes.NumDiscStates  - 
sizes.NumOutputs z 
sizes.NumInputs = 
sizes.DirFeedthrough = 
sizes.NumSampleTimes = 
sys = sinsizes(sizes); 
x0 =[1.0 0]; 

str =[]; 

ts =[l; 

function sys = mdlDerivatives(t, x,u) 
a = 1.2; 

b = 0.1; 

u tol-u(1); 


sys(1) = x(2); 

sys(2)= -a*x(2) - b* x(1) * u tol; 
function sys = mdlOutputs(t,x,u) 
sys(1) = x(1); 

sys(2) = x(2); 


(5) 作 图 程序 : chapl3, 3plot. m, 


close all; 


figure(1); 

subplot(211); 

plot(t,y(:,1), 'r',t,z(:, 1), 'b', 'linewidth',2); 
xlabel('time(s)');ylabel('q1 and zl response'); 
legend( 'q1', 'z1"); 
subplot(212); 
plot(t,y(:,2), 'r',t,z(:,2), 'b', 'linewidth',2); 
xlabel('time(s)');ylabel('q2 and z2 response'); 
legend('q2', 'z2'); 
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figure(2); 

subplot(211); 

plot(t,ut(:,1), 'r', linewidth',2); 
xlabel('time(s)');ylabel('control input at t'); 
subplot(212); i 
plot(t,ut(:,2), 'r', 'linewidth',2); 
xlabel('time(s)');ylabel('control input with delay'); 


附录 


针对 如 下 含有 参 变数 积分 限 的 积分 
plu) -| f (zu)dr 
plu) 
有 如 下 引 理 : 
" 
33 13.17 DRAR S EL d ERAT [a b] X [a B] 上 连续 ,函数 plu), 


quila, B] ETH, mE% aug 时 ,a 二 p(wu) 三 b .asq(G ) b. AA d EX 5x 
函数 y 在 [a,B] 上 可 微 , 而 且 


qtu) 9 i " 
J' hu ) -| y (Es u ) 


piu) Ju 


dx + f (qQu) ug Cu) — f Cp GO D p' u) 
&v-e| (fu ede) dse Ac - 'écoae Vw] Acoas 


L) k 9 
VvV=o| A CO | ataw DA Dr) 
i [9t dt dt 


—e| was 一 o| u’ de eo 


— wru! (t) -e[ 


jm 


u^ CE) dE 


其 中 , A( 一 zr) Sa-o-Aa-o-[ u^(E)d£, 
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基于 执行 饥 量 化 的 控制 


网 络 控制 是 控制 理论 的 发 展 热点 ,在 网 络 控制 中 ,信道 容量 约束 会 产生 量化 等 一 系列 问 
题 , 经 量化 后 的 系统 应 在 通信 速率 尽 可 能 小 的 情况 下 , 仍 能 保持 系统 稳定 并 满足 可 接受 的 控 
制 精 度 。 量 化 控制 系统 设计 通过 将 控制 与 通信 相 结合 来 解决 大 量 运 用 网 络 信息 技术 的 现代 
运动 系统 相关 控制 问题 。 采 用 量化 控制 方法 ,通过 将 控制 与 通信 相 结 合 , 可 以 解决 运用 网 络 
技术 进行 信号 传输 的 控制 问题 。 


14.1 基于 执行 器 容错 的 控制 输入 量化 控制 


14.1.1 系统 描述 

考虑 如 下 模型 
T, 
| (44. D 
i,—r 

H'ip.e—QGO.QGOZ e dilsis A u 的 量化 值 。 
采用 随机 量化 器 ""” 
QGO — round (7) (14. 2) 


其 中 ,k 为 量化 水 平 ,之 0。 
考虑 到 执行 器 存在 部 分 失效 的 情况 ,实际 控制 输入 为 
r—3QQ). Ņ E 0,1) (14.3) 
取 zi 的 指令 为 raS sint , 则 误差 及 其 导数 为 e=zi 一 sint,e 一 zz 一 cost。 控 制 目标 为 
位 置 及 速度 跟踪 , 即 当 :一 co 时 ,e 一 0,e 一 0。 


14.1.2 量化 控制 器 的 设计 与 分 析 


4 Qlu)=q, (t)u +q: GG) WR 


u(t) (14. 4) 
l; (a) |a 


t)) 
(4) Je s |ui |za 
qıt) = 
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"T 0, | u(t) | 之 a (14.5) 
PA E = e. 
es Q(GD—uQ, lu |a 


针对 量化 器 ,有 以 下 两 点 说 明 : 
COD 由 于 量化 过 程 符号 不 变 , 则 根据 式 (14. 3) 可 知 g (0770. 
(2) BUT? uoa 时 ,Q(w) 有 界 ,g1(t)= 二 1, 从 而 gq,(t) 有 界 , 可 取 |g,(1)| 二 gq;。 
取 滑 模 函 数 为 
s =ce +e 
其 中 ,ce 二 0。 
则 
$ —cé +e — (Q) —r, + cé — 93Q —Àx,-- ce — qiu + yq: 一 Zd4 十 ce 


=0u + 34: —T a 十 ce 
其 中 O= Nio 


s$ =s (Âu + mq: i 十 ce) — sÜu + sqq; + sCcé 一 zu) 


< sÜu 十 cis? + 2n 十 s(ce 一 zi) =s(— ls d- ls +cis Hce —x 4) + sôu + 
其 中 estes + 461270 用 于 调节 收敛 误差 。 
1 
取 z=ls+ Le =f ,250, Bj 
1 


<— Is? tsut sôu FL 


针对 上 式 分 析 如 下 : 

CD 由 于 20 未知, 为 此 要 对 0 进行 估计 。 

(2) 针对 时 变 参 数 0 二 oq, 的 自 适 应 估计 ,由 于 该 参数 无 法 求 导 ,需要 对 其 界 进行 
估计 。 


针对 时 变 增益 0 二 oq1, 取 一 ,其 中 ,0w 为 0 的 下 界 , 设 计 如 下 Lyapunov PÉ 
数 为 
ME, MM 


Hp, y>0,0 =p, H 020 n[ Al 420, 
Du 


box A 
Vost rh «C— [s? TT 
设计 控制 律 和 自 适应 律 为 


u 一 一 一 一 一 一 (14. 6) 


Ê — Ysu — Yap (14. 7) 
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其 中 ,po 二 0,a 二 0。 
则 
suu Í.xu 
V xC— Is! 4- su — 0 一 一 一 c —4i--—BuGOsu — Yap) 
| suu "e 4c, yu 
í a | ias 
BHFlal-— TERTIO , 则 一 rz spa 2T lel Spa Bt a —spu JU 
= m < p— suu 

p +| spu | 


考虑 到 92:0, = >O 


ER 

1 Ac 

一 入 x X < —(p — spu) 
| sġu Ia p H 


1 由 
V «— n5! suc (p — su) 十 本 ib lag lad 
7 p 7 


1 "A PN 
一 一 /5 Hsu ty ^p ni — psu) 一 —pap 
4 Hu 
"UD T li 
= 
Q nt pe Jus 
由 于 
n Ls 
TnS plate) =p Tp &—pu tsp tyr = sh tot 


V «— Is! TEST RTT Ms <i ob adi T d x—cV rd 
Cl u Hu 2 2p 


a AL Lt C min(2/ , ya}. 
1 
求解 不 等 式 V 壹 一 XV 十 d ,可 得 
"AES. 

«v « (vv - 7)e PT 

BUR BR np 48 
lim VG) S — 

可 见 , 如 果 A 足够 大 4d 足够 小 , 则 :一 ce 时 ,V(i) 一 0, 从 而 S 一 0,e 一 0,e 一 0。 


14.1.3 仿真 实例 


被 控 对 象 取 式 (14. 1) ,位 置 指令 为 ra= sint, KAERRA. 2) 实 现 控 制 输入 的 量 


化 ,k= 二 5. 0。 采 用 控制 器 式 (14. 32, 5X C14. 60 M L3 I f C14. 70, 5 (00) —0, X 
c—15,1—15,c, —10,0—0.02,a—0. 20, —2.0,5—0. 40, 仿 真 结 果 如 图 14-1 一 图 14-3 


所 示 。 
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xl tracking 


xld and x1 


dxld and x2 


time(s) 
14-1 状态 的 跟踪 


control input,ut 
e 


-50 
0 2 4 6 8 10 
time(s) 
& 50 
- 
z 
Q 
S0 
£ 
5 -50 
sa 0 2 4 6 8 10 
time(s) 


control input,tol 
o 


time(s) 


图 14-2 控制 输入 及 量化 输入 变化 
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Qu 


14-3 控制 输入 和 量化 输入 之 间 的 关系 


仿真 程序 如 下 : 
(1) Simulink 主 程序 : chapl4_lsim. mdl, 


chapí4 1ctrl 
Sine Wave chap14 1plant 


S-Functiont 


To Workspace 
(20 控制 器 子 程序 ; chapl4 lctrl. m, 


function [sys,x0, str,ts] = model(t, x,u, flag) 
switch flag, 
case 0, 

[ sys, x0, str, ts] = ndlInitializeSizes; 
case 1, 

sys = ndlDerivatives(t, x,u); 


case 3, 
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sys = mdlOutputs(t,x,u); 
case (1,2,4,9) 

sys-[]; 
otherwise 

error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys,x0,str,ts] = mdlInitializeSizes 
global c 1 


sizes = simsizes; 


sizes.NumContStates 


sizes.NumDiscStates 


sizes.NumOutputs 


sizes.NumInputs 


sizes.DirFeedthrough 


sizes. NumSampleTimes 
sys = simsizes(sizes); 

x0 = [0]; 

str = []} 

Es = [Jj 

c=15;l=15; 

function sys = mdlDerivatives(t, x,u) 
global c 1 

xd=u(1); 

dxd = cos(t);ddxd= - sin(t); 


x1 = u(2);x2 = u(3); 
e= xl - xd; 

de = x2 - dxd; 
s-c*etde; 


gama - 2; 
alfa = 0.20; 
cl = 2.0; 


u_bar= 1 * s + 1/c1 * s + c * de - ddxd; 


dmiu = gama * s * u bar - gama * alfa * x(1); 
sys(1) = dniu; 

function sys = mdlOutputs(t, x, u) 

global c 1 

xd=u(1); 

dxd = cos(t);ddxd = - sin(t); 


xl = u(2);x2 = u(3); 
e-xl- xd; 

de = x2 — dxd; 
s-c*etde; 


c1-722.0; 
u_bar = 1 * s + 1/c1 * s + c * de- ddxd; 
rho = 0. 020; 
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ut= -s*x(1)^2*u bar^2/(abs(s * x(1) * u bar) + rho); 


k=5; 
Qu= k * round(ut/k) ; 


xite - 0.40; 
tol- xite * Qu; 


sys(1) = ut; 
sys(2) = Qu; 
sys(3) = tol; 


(3) 被 控 对 象 子 程序 : chapl4_lplant. m, 


function [sys,x0, str,ts] = s function(t, x, u, flag) 
switch flag, 
case 0, 
[sys, x0, str, ts] = ndlInitializeSizes; 
case 1, 
sys = ndlDerivatives(t, x,u); 
case 3, 
sys = mdlOutputs(t,x,u); 
case (2, 4, 9 } 
sys = []; 
otherwise 
error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0, str, ts] = mdlInitializeSizes 
sizes = simsizes; 


sizes.NumContStates 


sizes.NumDiscStates 
sizes.NumOutputs 


sizes.NumInputs 
sizes.DirFeedthrough 
sizes. NumSampleTimes 


" 
Oo 忆捷 局 也 
` 


sys 7 simsizes(sizes); 

x0 -[0.150 0]; 

str -[]; 

ts =[]; 

function sys = mdlDerivatives(t, x,u) 
Qu - u(1); 

sys(1) = x(2); 

sys(2) = Qu; 

function sys = mdlOutputs(t, x, u) 
sys(1) = x(1); 

sys(2) = x(2); 


(4) 作 图 子 程序 ; chapl4 lplot. m, 


close all; 
figure(1); 
subplot(211); 
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plok(t, x(:,1),'r',t,x(:,2), "o "o 'Linewidth!',2); 
xlabel('time(s)');ylabel('xld and x1'); 
legend('x1l tracking'); 

subplot(212); 

plot(t,cos(t), 'r',t,x(:,3), ':', 'linewidth',2); ' 
xlabel('time(s)');ylabel('dxld and x2'); 
legend('x2 tracking'); 


figure(2); 

subplot(311); 

plot(t,ut(:,1), 'r', 'linewidth',2); 
xlabel('time(s)');ylabel('control input, ut'); 
subplot(312); 
plot(t,Qu(:,1), 'r', 'linewidth',2); 
xlabel('time(s)');ylabel('control input,Qu'); 
subplot(313); 
plot(t,tol(:,1), 'r', 'linewidth',2); 
xlabel('time(s)');ylabel('control input, tol'); 


figure(3); 
plot(ut(:,1),Qu(:,1), 'r', 'linewidth',2); 
xlabel('ut'); ylabel( 'Qu'); 


14.2 基于 状态 观测 器 的 输入 量化 反馈 控制 
14.2.1 系统 描述 


考虑 如 下 模型 
Ti ST? 
Te =r (14.9) 
YTTI 


其 中 ,t= 二 Q(u) ,Q(z) 为 控制 输入 的 量化 值 。 
采用 随机 量化 器 2 


QC) —óround(--) (14. 10) 
其 中 ,6 为 量化 水 平 ,6 二 0。 


针对 输入 量化 问题 ,假设 只 考虑 输出 zi 为 可 测 ,实际 控制 输入 为 + 二 Q(u),u 为 待 设 
计 的 控制 输入 。 取 x 二 [x， zc ,控制 目标 为 :00 时 ,x 一 0。 


14.2.2 控制 器 的 设计 与 分 析 


引 理 14.1 针对 量化 器 Q(z), 有 
| Q(z)—z |S] z | (14. 11) 
51 14.2 ”对 于 正定 和 矩阵 P, HI P,.A—Lc! 和 A 一 bK" 满足 Hurwitz 条 件 , 则 
(A — Lc')' P, +P, (A — Lc) 2 —Q, 
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(A — bK')' P, CP, — bK") =—Q, (14. 12) 
其 中 ,O, MO, 为 正定 矩阵 ,A L. b.c KEIM 14.2 和 式 (14. 19), 
设计 状态 观测 器 
Z, =2,; +l (zı =z) 
: (14. 13) 
Z: =v +l:(xı —3,) 
XE XXL a3 2$ L — [L UG]. 
设计 控制 律 为 
u =— Kx 
(14. 14) 
c —Q(u) 
其 中 ,控制 增益 K=[k R]jy.x-[ rl. 
W e—x—x.llle,—r,—2,.e;—r,—2,.H 
sn 
e; =Q(u)— u — l;e, 
Bp 
el = [i 1 £1 0 
交加 | Ep f eo -o 
e» — 0 €» 1 
O0 1] L =j 1 
由 于 4 一 Ler=| ]- o ü-| EL 
0 0 l; = 0 
e —(A —Lc)e -b(QGO — uw) (14. 15) 
0 ] 0 ] 
* = kJ zi s m asi *, = o 
其 中 ,A | [2 ME lo 
m 0 1 
AFLG =e E| MEL 
(14. 16) 


$ —(A — bK") + Le, 
定义 闭环 系统 Lyapunov 函数 为 
V=V, +V, 
为 了 实现 e 一 0, 定 义 第 一 个 Lyapunov 函数 
V, —e'P,;e 
其 中 ,P, 为 对 称 正 定 矩 阵 。 


由 于 
eIPie 十 eIPie= [e' (A — Lc)! J-(QG) —wb! JP,e 3- e! PL[CA — Lee +b(Qlu)—u)] 


—e"'[(A — Lc')*P, -- P,(A — Lc?) e -2e' P,b(QQGQu) — u) 


根据 引 理 14. 2, 可 得 


V, ——e'Q,e +2e'P,b(Qlu)—u) (14.17) 


为 了 实现 x 一 0, 定 义 第 二 个 Lyapunov 函数 


V,=x'P,x 
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其 中 ,P， 为 对 称 正定 和 矩阵。 
则 根据 引 理 14. 2, 可 得 


V, = Y P,X +TP, to((A—bK RT+Le) PX ip,((A— bK)E+Le,) 
=x'[(A—bK')'P,+P,(A—bK')]x+2x'P,Le, =—xX'Q,x + 2x! P)Le, 


(14, 18) 
由 于 
2x'P,Le, <S || x l* +| PL I? lel 
根据 引 理 14. 1, 可 得 
Qluj—u S| u |=|— kı —h;z; l| biz, [H| kz? |=| KTF |< IKI M x dI 


则 
2e'P,b(Qlu)—u) <2 || P,b ll lel IKI ll < I Pl IKI dl? +l el?) 
式 (14. 17) 和 式 (14. 18) 变 为 
V, «—2A,.Q)DIel^- lP,bll IKIE + Pol IKI lel? 
V, L-A ma (QD EHIE HIP N? lel? 
HRA nin O, ) 为 Q: 的 最 小 特征 值 ,.M min C Q0 79 Q: 的 最 小 特征 值 。 
则 
V —V,4V, 
«—AQlel*-1P»l KI lxli*-IPl KI lell?— 
Ass (Q2 x 二 + Lx l* E PSL I? Hel? 
=— (À min (Q1) — IPibl IKI —IP;LI?»lel*—(,,€09)— 
IPI Kl —DlxIll* 


选取 K 4 L.fü FAX 
À min (Q2) — lI P,b ll | KI —12620 


, (14. 19) 
Amin (Q) — lI P451 ll KI —l PL ||? 2620 
则 
V «&—eClell*--lxl^»so 
根据 V=e "Pie 十 x*'P, x. 
V « A, Gi lel*--A,,€51xl?sgclel*?--xl^ 
HP, p= maxlA (GAS 2). 
从 而 
—gCIGlel*-- lxl^»s«-v 
Ve—ec(Clel*-4- xl? 一 一 二 8 lell*-- zl x-—Lv 
B B 
由 于 不 等 式 V 过 一 aV HRIVA Ke VO), WME a 为 正 实数 , 则 V(z) 以 指数 形式 收 
ATE. Wu 


VG) & e" VQ) 
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闭环 系统 为 指数 收敛 , 即 当 1>co 时 ,e>0,%>0 且 指 数 收 但 ,从 而 当 ; -co 时 xz， 一 0， 
2,0 且 指 数 收 化 。 系 统 的 收 化 速度 取决 于 e 和 有。 


14.2.3 仿真 实例 


被 控 对 象 取 式 (14. 9) ,采用 量化 器 式 (14. 10) .观测 器 式 (14. 13)、 控 制 律 式 (14. 14) , 观 
测 器 初始 状态 取 x*(0) 王 [0.1 0.2], 被 控 对 象 初始 状态 取 x(0)= 王 [0.5 0.5]. 

控制 器 取 & — 300.4, = 二 30, 观 测 器 取 4 二 10,l, 二 20, 控 制 输入 量化 器 取 5==15, 仿 真 结 
果 如 图 14-4 一 图 14-7 所 示 。 


0.6 


二 "| 
| 
0 


-0.2 
5 10 15 20 


time(s) 


x2 response 


NENNEN 
0 5 10 15 20 


time(s) 
图 14-4 状态 的 响应 


designed control input.vt 
M 
S 


0 5 10 15 20 
time(s) 


quantised input,ut 
M 
e 


5 10 15 20 
time(s) 


图 14-5 控制 输入 及 量化 输入 变化 
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-40 -30 -20 -10 0 10 20 
vt 


图 14-6 ”控制 输入 和 量化 输入 之 间 的 关系 


0 5 10 15 20 
time(s) 
图 14-7 观测 器 观测 结果 


仿真 程序 如 下 : 
(CD JF: chap14_2sim. mdl, 


chap14_2plant 


S-Function1 


chap14 2obv 
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(2) 控制 器 程序 : chapl4 2ctrl. m, 


function [sys,x0,str,ts] = model(t,x,u,flag) 
switch flag, 
case 0, 

[ sys, x0, str, ts] = ndlInitializeSizes; 
case 3, 

sys = mdlOutputs(t, x, u); 
case (1,2,4,9) 

sys- []; 
otherwise 

error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0,str,ts] = mdlInitializeSizes 
sizes = simsizes; 
sizes.NumDiscStates  - 
sizes.NumOutputs = 


sizes.DirFeedthrough = 


0 
2 
sizes.NumInputs = 2; 
1 
sizes. NumSampleTimes = 0 


sys = simsizes(sizes); 


x0 = []; 
str = []; 
tg = []; 


function sys = mdlOutputs(t, x, u) 
k1 = 300;k2 = 30; 


xlp= u(1);x2p=u(2); 
vt = —kl* xlp-k2* x2p; 


delta = 1.0; 
ut = delta * round(vt/delta); 


sys(1) » vt; 
sys(2) = ut; 


(3) 观测 器 程序 : chapl4 2obv. m, 


function [sys, x0, str,ts] = model(t,x,u, flag) 
switch flag, 
case 0, 

[sys, x0, str, ts] = ndlInitializeSizes; 
case 1, 

sys = ndlDerivatives(t,x,u); 
case 3, 

sys = ndlOutputs(t,x,u); 
case {1,2,4,9} 

sys-[]; 
otherwise 

error(['Unhandled flag = ',num2str(flag)]); 
end 
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function [sys, x0, str,ts] = mdlInitializeSizes 


sizes - simsizes; 


oO -r-e & tN Oo Y 


sizes.NumContStates 
sizes.NumDiscStates  - 
sizes.NumOutputs - 
sizes.NumInputs - 
sizes.DirFeedthrough - 


sizes.NumSampleTimes 
Sys = simsizes(sizes); 

x0 [0.1 0.2]; 

str = []; 

ts = []; 

function sys = ndlDerivatives(t, x,u) 
xl-7u(1); 

x2 = u(2); 

y7xl; 

qy =y; 


delta = 0.05; 
qy = delta * round( y/delta); 


vt=u(3); 


11 = 10;12 = 20; 
xlp=x(1);x2p= x(2); 


sys(1) =x(2) * 11 * (qy— x1p); 
sys(2) » vt * 12 * (qy- x1p); 
function sys = mdlOutputs(t, x, u) 
xlp-x(1);x2p- x(2); 

sys(1) = xlp; 

sys(2) = x2p; 


(4) 作 图 程序 : chapl4 2plot. m, 


close all; 

figure(1); 

subplot(211); 
plot(t,x(:,1), 'r', 'linewidth',2); 
xlabel('time(s)');ylabel('xl'); 
legend('xl response'); 
subplot(212); 
plot(t,x(:,2), 'r', 'linewidth',2); 
xlabel('time(s)');ylabel('x2'); 
legend('x2 response'); 


figure(2); 

subplot(211); 

plot(t,ut(:,1), 'r', 'linewidth',2); 
xlabel('time(s)');ylabel('designed control input, vt'); 
subplot(212); 
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em 


plot(t,ut(:,2), 'r', 'linewidth',2); 
xlabel('time(s)');ylabel('quantised input, ut'); 


figure(3); 
plot(ut(:,1),ut(:,2), 'r', 'linewidth',2); 
xlabel('vt'); ylabel('ut'); 


figure(4); 

subplot(211); 

plot(t,x(:,1), 'k',t,xp(:,1), rw '1inewidth',2); 
xlabel('time(s)');ylabel('x1 and xlp'); 
subplot(212); 

piot(t,x(:,2), 'k', t, xp( *, 2), T "1isewadth!, 2); 
xlabel('time(s)');ylabel('x2 and x2p'); 


14.3 基于 状态 观测 器 的 输入 输出 量化 反馈 控制 


14.3.1 系统 描述 
考虑 如 下 模型 


N —X9 
B, =u (14, 20) 


三 XI 

假设 只 有 zi 为 可 测 , 同 时 考虑 输入 输出 量化 ,模型 的 输出 量化 为 g(y) ,控制 输入 量化 
A u-qG) v 为 待 设计 的 控制 输入 。 

量化 器 定义 如 下 "3 


a(z) =ðround(Ž) (14. 21) 


其 中 ,6 为 量化 水 平 ,8 二 0。 
B x-[z, x] ,控制 目标 为 上 一 ce 时 ,x 一 0。 


14.3.2 控制 器 的 设计 与 分 析 


引 理 14.3 针对 g(z), 有 
Iq(z2—z|xlz| (14. 22) 
引 理 14.4 ”对 于 正定 矩阵 P, HI P,.A—Lc! 8l A—bK' 满足 Hurwitz 条 件 , 则 


(A —Lc')'P, +P, (A — Lc") =— Q, 
| (14. 23) 
(A—bK')'P,+P,(A—bK')=—0, 
其 中 ,Q, fl Q; 为 正定 矩阵 ,A ,L ,b,c ,K BF EX. 
设计 状态 观测 器 为 
zi 一 Zr li(q(y) —z1) 
. (14. 24) 
3, =v +l,(q(y)—2,) 
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定义 观测 器 增益 工 二 [1， 0L] .L 满足 引 理 14. 4。 
设计 控制 律 为 
v —— Kx 
— (D 
其 中 ,控制 增益 KK 二 [&， kisi, 2.] ,天 满足 引 理 14.4. 
取 e—x—x,We;—x,—2z,.e:—2x;—2,.H 
e; =e, —l,(qGy) —2)) =e, 一 由 (Gy) 一 >y 十 y 一 z)) 
一 一 人 el 十 ez 一 个 (qGy) 一 y) 
é;—u-—v-—l;(q)—2,))—u—v-—l,(q)—ycy-—uü) 
—q(v)—v-—l;e,—ls(qCBy2)— y) 
从 而 
Mofes 


ei —qG) —v — le, — li (q(32 — y) 
即 


^ cs Sp eI aes ;+ [| «co ) 
n i 9i i ^ ^ dB -— 
o n fu —hod 
A citu = l Bye ,从 而 
ET b i Wb J " à 
je: (E — Ee T)a — Lügt gy) — 33-H- Digi) —2) 


0 1 CM NEM 
其 中 ,A 一 | 中 = l5] a- ple we 


~ ^ 0 
BUT LGGO—zr)-—L(G,;—z,ctqG)0—y)-Le,;tL(qCy) —3y).H he 
1 


A — bK' , 则 
€ —(A —bK'O£ Le, -L(GQG0 — y) 
定义 闭环 系统 Lyapunov KXON 
V=V, +V: 
为 了 实现 e 一 0 ,定义 第 一 个 Lyapunov 函数 
V4 一 eIPie 
其 中 ,P, 为 对 称 正 定 矩 阵 。 
由 于 


&' P,e - e'P,é = [e (A — Lc')' — (q(y) — y)L* + (qv) —v)b*] Pie 十 


e' P, [CA — Lc')e — L(qCy) — y) - b Cv) — v)] 


(14. 25) 


(14. 26) 


(14. 27) 


—e! [XA — Lc! )'P, -- P,CA — Lc)]e —2e' P, L(qCy) — y) + 


2e! Pb(qGv) — v) 
根据 引 理 14. 4, 可 得 
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V, ——e'Qi,e — 2e' P,L(QG) — y) -2e!P,b(qGo) — v) (14. 28) 
为 了 实现 x 一 0, 定 义 第 二 个 Lyapunov 函数 
V. —x'P.x 


其 中 ,P; 为 对 称 正定 矩阵 。 
则 根据 引 理 14. 4, 可 得 
V,— E PX HTP, -—A BK Le, L(G(G) — y)) PX + 
x'P,(CA —bK'ox t Le; +L(q(y)— y» 
— x' [(A — bK')'P, 4- P,CA — bK')] x --2x' PLe, 4- 2x P,L(qCBy) — y) 
—x'Q.x --2X' P,Le, - 2x P,L(q Gy) — y) 


则 
2x P,Le < | x ||? +I P;L ||? lel? 
根据 引 理 14. 3, 可 得 
gly) ~—y S| y I5 x |=|z1+e ls HH es | 
qG)—vx| v |=|— k; — k:t || bim, | 十 | hx; |=| K*x |< HK IE l£ I 
由 于 
':2]xlcz He (|)=2| x lz |+2|xl|e | x: 十 
[2: | 十 中 站? 十 | ei lI 8l x ll* e | 
2 lell dz, l+ e Ds lel? +42 | 十 | er D? 
= |e l? +: +2 lz Ile, IHl e l? 
< llel? +2 cz, [H ei PH e l? 
<3 lell’ +212, |” 
则 


—2e'P,L(GG) — y) &:2| PL || lel Clzi H ei D 3l PLI lell*--21P,LIlz l 
2x P4L (Cy) — y») «C 2| P4L V lx ll Clg, ltle D 3l PLI lx l* PL I Les l? 


2e'P,b(qCo) —v) «2| Pb ll lel lKI lxi s IPbl IKIcCIxl*- lel 
从 而 有 
V, 委 一 MnO lel? +3 PLI lel*--21P,LITZ, I I P5 ll KI Ix M? 


Pb KI llel? 

< (GIPLI + Pl IKI»lIxl*-Ca4QD-c3l PLI 4 IPD OLK IDO Ile ll? 
V,s—ASWQIxl*-Izl*--IPLII*Iel*o3lP;LI EI+ IPL | l e l 

= (— À mn (Q2) +1 +3 || P;LI»Ixl*-cIP;Ll*-- IPL IO lell? 
其 中 Ass (Q0 78 Q: 的 最 小 特征 值 ,Xi,(Q1) 为 Q, 的 最 小 特征 值 。 

则 
V=Vi+V; «— (2 | PLI — lP;bl | KI HAm) — 1—3 IPL I) il — 

Amin (Q) —31l P.L ll — ll Pb ll K || —I PL I? — lI PL IlO lell? 
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选取 K 和 天, 使 如 下 不 等 式 成 立 

A.u(Q) —2l PLI —3l PEL — E Pb M IKI —12620 

A.s09;2—3[| PL Il — LPE I —lP;Lil*— Pl KI 2:90 

(14. 29) 
则 
V«-eClel*- l£l5«o 
RHE V—e'P,e--x' P, x.t 
V S AG; lel ^ A4, lx l* s gcllel*-- lix l^ 
其 中 ,8 二 max {À max (P1) sÀ ma (O2). 

由 于 一 8( e ll^4- lx ls —V s Bu 


V«-eClelt I£I0-—sgClell*- 1$ <- FV 


由 于 不 等 式 V 受 一 ay HRIVA LVO), WE a 为 正 实数 , 则 VCz) 以 指数 形式 收 
AFE. I 


Va) e? V» 
闭环 系统 为 指数 收敛 , 即 当 1 一 co 时 ,ee 一 0,x 一 0 且 指 数 收敛 ,从 而 当 z 上 一 ce 时 ,一 0， 
r,—0 且 指 数 收 僵 。 系 统 的 收敛 速度 取决 于 B flle. 


14.3.3 仿真 实例 


被 控 对 象 取 式 (14. 20) ,被 控 对 象 初始 状态 取 x(0)= [0.5 0.5]。 
采用 量化 器 式 (14. 21) 观测 器 式 (14. 24) 、 控 制 律 式 (14. 25), 观 测 器 初始 状态 取 xX (0) = 
[0.1 0.2]。 控 制 器 取 上 ,= 二 300,k, 二 30, 观 测 器 取 7, = 二 10,1, 二 20, 控 制 输入 量化 器 取 5= 


0.02, 输 出 量化 器 取 65==0.01, 仿 真 结果 如 图 14-8 一 图 14-11 所 示 。 
0.6 


time(s) 
14-8 ”状态 的 响应 
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图 14-9 控制 输入 及 量化 输入 变化 
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图 14-10 ”控制 输入 和 量化 输入 之 间 的 关系 


time(s) 
14-11 观测 器 观测 结果 
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S 

X- 
3 
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-3 

0 2 4 6 8 10 
time(s) 
图 14-11. (2D 
仿真 程序 如 下 : 


CD JF: chapl4 3sim. mdl。 


chap14_3plant 


chap14 3ctri 


chapi4 3obv 


Clock To Workspace 


(2) 控制 器 程序 : chapl4 3ctrl. m, 


function [sys,xO,str,ts] = model(t, x,u, flag) 
switch flag, 
case 0, 

[sys, x0, str, ts] = mdlInitializeSizes; 
case 3, 

sys = mdlOutputs( t, x,u); 
case {1,2,4,9} 

sys - []; 
otherwise 

error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0, str, ts] = ndlInitializeSizes 


Sizes - simsizes; 


sizes.NumDiscStates 0; 


ii 


sizes. NumOutputs 2; 
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sizes.NumInputs = 2; 


sizes.DirFeedthrough T4 
Sizes.NumSampleTimes 


u 
o 
T 


SyS 7 simsizes(sizes); 


xü = []; 
str = []; 
ts = [i]; 


function sys = mdlOutputs(t, x, u) 
k1 = 300;k2 = 30; 


xl =u(1);x2=u(2); 
vt- -kl*xl-k2* x2; 


delta = 0.02; 
ut = delta * round(vt/delta); 


sys(1) » vt; 
sys(2) = ut; 


(3) 被 控 对 象 程序 : chapl4 3plant. m, 


function [sys, x0, str, ts] = s function(t, x, u, flag) 
Switch flag, 
case 0, 
[ sys, x0, str, ts] = ndlInitializeSizes; 
case 1, 
sys = ndlDerivatives(t, x,u); 
case 3, 
sys = mdlOutputs(t,x,u); 
case (2, 4, 9 } 
sys = []; 
otherwise 
error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0, str,ts] = mdlInitializeSizes 
sizes = simsizes; 


sizes.NumContStates 


sizes. NumDiscStates 
sizes. NumOutputs 


sizes.NumInputs 
sizes.DirFeedthrough 
sizes.NumSampleTimes 


LU 
= ot NO t 
T 


SyS 7 simsizes(sizes); 

x0 -[0.50.5]; 

str -[]; 

ts ={]; 

function sys = ndlDerivatives(t, x,u) 
ut-u(1); 

sys(1) = x(2); 

sys(2) = ut; 

function sys = mdlOutputs(t, x, u) 
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sys(1) = x(1); 
sys(2) = x(2); 


(4) 观测 需 程 序 : chapl4 3obv. m, 


function [sys,x0, str,ts] = model(t,x,u, flag) 
switch flag, 
case 0, 

[sys, x0, str, ts] = mdlInitializeSizes; 
case 1, 

sys = ndlDerivatives(t,x,u); 
case 3, 

sys = ndlOutputs(t,x,u); 
case {1,2,4,9} 

sys- []; 
otherwise 

error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0, str,ts] = mdlInitializeSizes 


Sizes - simsizes; 


sizes.NumContStates 


a 
O m^ A NORN 
S 


sizes.NumDiscStates 


sizes. NumOutputs 


sizes.NumInputs 


sizes.DirFeedthrough 


sizes.NumSampleTimes 


Sys = simsizes(sizes); 


x0 - [0.10.2]; 
str = []; 
ts = |l; 


function sys = mdlDerivatives(t, x,u) 
xl =u(1);x2 = u(2); 


delta= 0.01; 
y= delta * round(x1/delta); 
vt-u(3); 


117 10;12 = 20; 
xlp= x(1);x2p= x(2); 


sys(1) = x(2) + 11 * (y- x1p); 
sys(2) " vt * 12* (y- x1p); 
function sys = mdlOutputs(t,x,u) 
xlp-x(1); 

x2p- x(2); 

sys(1) = xlp; 

sys(2) = x2p; 


(5) 作 图 程序 : chapl4 3plot. m, 


close all; 
figure(1); 


[1] 
[2] 
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subplot(211); 

plot(t,x(:,1), 'r', 'linewidth',2); 
xlabel( 'time(s)');ylabel('x1'); 
legend('x1l response'); 
subplot(212); 

plot(t,x(:,2), 'r','linewidth',2); 
xlabel('time(s)');ylabel('x2'); 
legend('x2 response'); 


figure(2); 

subplot(211); 

plot(t,ut(:,1), 'r', 'linewidth',2); 
xlabel('time(s)');ylabel('designed control input, vt'); 
subplot(212); 

plot(t,ut(:,2), 'r', 'linewidth',2); 
xlabel('time(s)');ylabel( 'quantised input, ut'); 


figure(3); 
plot(ut(:,1),ut(:,2), 'r', 'linewidth',2); 
xlabel('vt');ylabel('ut'); 


figure(4); 

subplot(211); 

plot(ít,x(:, 1), 'k", t,xp( :, 1) , tr's '1inewidth",2); 
xlabel('time(s)');ylabel('x1 and x1lp'); 
subplot(212); 

plot(t,x(:,2), 'k',t,xp(:,2), 'r', 'linewidth',2); 
xlabel('time(s)');ylabel('x2 and x2p'); 
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基于 控制 方向 未 知 的 控制 


CHAPTER 15 


控制 方向 未 知 的 问题 是 一 个 有 意义 的 控制 问题 。 当 系统 中 存在 未 知 控制 方向 时 ,会 使 得 
控制 器 的 设计 变 得 复杂 ,Nussbaum 增益 技术 是 处 理 控制 方向 未 知 问题 的 一 种 有 效 方法 。 


15.1 基本 知识 


EXU WFAA N (X) 满 足下 面条 件 , 则 N OO XJ. Nussbaum 函数 ,简称 N 函数 。 
Nussbaum 函数 满足 如 下 双边 特性 


] fx 
lim sup x] N (s)ds — co 
kd 0 


k 
lim inf il N (s)ds =— ĉo 
kæto k 0 


根据 Nussbaum MÄGE X. ,定义 Nussbaum PR Zt Jg 
N (k) =k°cos(k) (15. 1) 
其 中 ,k 为 实数 。 
3|38 15.1 如 果 VG) 和 k&(，) 在 VtEL[0,4j) 上 为 光滑 函数 ,V(t) 宇 0,N(。) 为 光 
滑 的 N 函数 ,0 为 非 零 常数 ,如 果 满 足 


VG) < [GN G GOD + DÉGOde + const Vz € [0,2,) 
WI VCO GR | G,NG GO) - DEGOde f£ Vr € [0,70 EER. 


8|38 15. 2( BI Barbalat 3| EO 如果 EL. f €Lb. H FEL, p ELL) H4 
t>o}, f 02-0, 


15.2 基于 控制 方向 未 知 的 状态 跟踪 


15.2.1 系统 描述 
被 控 对 象 为 


Xi =X? 
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z,-—0u--d (15. 2) 


其 中 ,9 为 正 负 符号 未 知 的 有 界 常 数 ,d 为 扰动 ,|d ID. 


则 


B za 为 指令 信和 号 ,控制 目标 为 1 一 co 时 $E X sti T4. 
15.2.2 控制 律 的 设计 
定义 跟踪 误差 为 € 一 六 1 — 32544 Bi] e 一 zy 一 Zu 定义 滑 模 函数 为 Y 一 ce 十 e „c>œ>0, Mi 


$ =ce +e 一 ce Hr, ~ta = cé 4-0u +d —x, 
定义 Lyapunov 函数 


Vs 


. 


V =ss =s (ce -- Qu +d (() 一 Zi) 


取 
a 一 Ais 十 ce 一 zi 十 7Ssgns， Ai 二 0，7 过 DD 二 7， 加 之 0 3 
7 ta hak (15.4) 
u =N(k)u CT5. 8) 
k—ysu, Y 0 (15. 6) 


根据 文献 [1-2], 式 (15.5) 中 的 N 函数 可 取 N (C) — RI cosCED 。 


ws d. - 
由 上 面 定 义 可 知 ce —z4=a—kis—ysgns s-k =su =s(—k,s +a), Ii] 
: i 1: Is 
V —5s(a — kis — q3sgns +d TENSEHU EE 


=s(a — kıs +0N (k)u)— q | s |+ sd E — SC kes +a) 


« s(a — k,s #ON(k)ū) 十 i R ys ed 
则 
V & «ON Ow + Rt Gs — ki)s’ 
NEC 
由 于 su—— , M] 
Y 
` 1 . l: " 
V & VON Gk Hk + Gs — Ost 
两 边 积 分 可 得 
VG) — VC) <| TINE EDRO  [ Tide — | Ck —ü d tede 
即 


VG) +f (ks — h Cedde &E f (ON Gk Cc) + DE GOdz +V (0) 
0 0 
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根据 引 理 15.1, vao Cka —k Ds? GO de 有 界 , 则 sj s! di 有 界 , 则 由 引 理 15. 2(Bar balat 
5| 385,24 t£ — eo Bus — 0, 从 而 e — 0,6 — 0, 


15.2.3 仿真 实例 


被 控 对 象 取 式 (15. 2) ,对 象 的 初始 状态 为 L0.2,0],& — sint ,0— 10, fr HS N zy 一 
sint, 采 用 控制 律 式 (15.3) 一 式 (15.5) 和 自 适 应 律 式 (15.6), 取 c 王 10,&(0) 王 1.0,A, 王 1， 
ks, 一 1.5,7 二 10,y 二 1.1, 仿 真 结果 如 图 15-1 和 图 15-2 所 示 。 


1 


on 
& 
E 0 
E 
S 
= -l 
& 
-2 
2 4 6 8 10 12 14 16 18 20 
time(s) 
2 
2g! 
-x 
Bo 
i. 
in 2 4 6 8 10 12 14 16 18 20 
time(s) 
图 15-1 位 置 和 速度 跟踪 
50 
40 
30 
&. 20 
£ 
E 
E 10 
0 
-10 
-20 


2 4 6 8 10 12 14 16 18 20 
time(s) 


图 15-2 控制 输入 
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仿真 程序 如 下 : 
(1) 输入 信号 程序 : chapl5 linput. mdl。 


function [sys,x0, str,ts] = spacemodel(t, x, u, flag) 
switch flag, 
case 0, 
[sys, x0, str, ts] = ndlInitializeSizes; 
case 3, 
sys = mdlOutputs(t,x,u); 
case (2,4,9) 
sys-[]; 
otherwise 
error([ 'Unhandled flag = ',num2str(flag)]); 
end 


function [sys, x0, str, ts] = mdlInitializeSizes 


Sizes = simsizes; 


sizes.NumContStates 


sizes.NumDiscStates 
sizes.NumOutputs 


sizes.NumInputs 
sizes.DirFeedthrough 
sizes.NumSampleTimes 


Li 
Í!|oonu"eosoco 
fy 


Sys 7 simsizes(sizes); 


x0 = [E] 
str = []; 
ts - [00]; 


function sys = mdlOutputs(t, x,u) 
sys(1) = sin(t); 


(2) Simulink 主 程序 : chapl5 lsim. mdl。 


Clock To Workspace 
(3) 被 控 对 象 S PX: chapl5 lplant. m, 


function [sys,x0, str,ts] = spacemodel(t,x, u, flag) 
switch flag, 
case 0, 
[sys, x0, str, ts] = ndlInitializeSizes; 
case 1l, 


390 Al 机 器 人 控制 系统 的 设计 与 MATLAB 仿 真 ， 基 本 设计 方法 (第 2 版 ) 


sys = ndlDerivatives(t,x,u); 
case 3, 

sys = mdlOutputs(t,x,u); 
case {2,4,9} 

sys- []; 
otherwise 

error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0, str,ts] = ndlInitializeSizes 


sizes = simsizes; 


sizes. NumContStates 


Ns 


sizes.NumDiscStates 


`~ 


sizes. NumOutputs 


sizes. NumInputs 


€T 


sizes.DirFeedthrough 


T 


LU 
= Boc DON 
EM 


sizes.NumSampleTimes 


`~ 


sys = simsizes(sizes); 

x0 [0.20;0]; 

str = [DL 

ts - [00]; 

function sys = ndlDerivatives(t, x,u) 
ut-u(1); 

dt = sin(t); 


th- - 10; 
% th= 10; 


sys(1) =x(2); 

sys(2) = th* ut * dt; 

function sys = mdlOutputs(t, x, u) 
sys(1) = x(1); 

sys(2) = x(2); 


(4) 控制 器 的 S 函数 : chapl5 lctrl. m, 


function [sys,x0, str,ts] = spacemodel(t,x,u, flag) 
switch flag, 
case 0, 
[sys, x0, str, ts] = ndlInitializeSizes; 
case 1, 
sys = ndlDerivatives(t,x,u); 
case 3, 
sys = ndlOutputs(t,x,u); 
case (2,4,9) 
sys- []; 
otherwise 
error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0, str,ts] » mdlInitializeSizes 
Sizes = simsizes; 


sizes.NumContStates  - 1; 


sizes.NumDiscStates 
sizes.NumOutputs 
sizes. NumIĪnputs 


sizes.DirFeedthrough 
sizes. NumSampleTimes 


SyS 7 simsizes(sizes); 


x0 -[1.0]; 
str = []; 
ts - [00]; 


function sys = mdlDerivatives(t, x, u) 
xd- u(1);dxd = cos(t);ddxd-» - sin(t); 


x1 = u(2);x2 = u(3); 


c 7 10; 
e-xl- xd; 
de = x2 - dxd; 


s-c*etde; 


k1-21;k2-71.5; 
xite = 0.10; 


alfa = k1 * s + c * de- ddxd + xite * sign(s); 


ub= -k2*s-*alfa; 


gama = 10; 
dk = gama * s * ub; 


sys(1) = dk; 


function sys = mdlOutputs(t, x, u) 
xd = u(1);dxd-7 cos(t);ddxd- - sin(t); 


xl = u(2);x2 = u(3); 


c=10; 
e-xl- xd; 
de = x2 - dxd; 


s-c*etde; 


k1-71;k271.5; 
xite-7 1.1; 


alfa = k1 * s +c * de- ddxd + xite * sign(s); 


ub= -k2 * s + alfa; 
k=x(1); 

Nk = k^2 * cos(k); 
ut = Nk * ub; 


sys(1) = ut; 


(5) 作 图 程序 : chapl5 l1plot. m, 


close all; 
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figure(1); 

subplot(211); 

plot(t,sin(t),'r',t,y(:,2),"— k", "Hnewidth', 2); 
xlabel('time(s)');ylabel('position tracking'); 
subplot(212); ' 


plot(t,cos(t), 'r', t, y(:,3), ' - . k', 'linewidth',2); 
xlabel('time(s)');ylabel('speed tracking'); 


figure(2); 


plot(t,ut(:,1), 'k', 'linewidth',2); 
xlabel('time(s)');ylabel('control input'); 


15.3 控制 方向 未 知 的 反 演 控制 


15.3.1 系统 描述 
被 控 对 象 为 


其 中 ,0 为 正 负 符号 未 知 的 有 界 常 数 。 
控制 目标 为 ti 一 co 时 ,zl 一 0, 并 一 0。 


15.3.2 控制 律 的 设计 及 分 析 


(15. 7) 


针对 式 (15.7) 的 结构 ,需要 采用 反 演 设计 的 控制 方法 。 定 义 zi 二 xz, 设计 Lyapunov PR 


数 为 V, — et 


V, =z] =z; (z: 3-3) 
取 xz, 二 zx, 一 Qi Qi 为 虚拟 项 , 则 
V, =z; z =z; (z: +a, +z) 
令 q4— —c,z1— z J 


Mj 9 
Ve 二 Zi 


5E X. Lyapunov K% V, —V, ts , 则 


V, 一 一 cljz! 十 zizy 十 zy (0u 一 ai) 
— —c,zi +z, (0u — à, 十 zl) 
设计 控制 律 为 
u —vw(kE)(z,—a,-4-zi) 
则 
V, ——c,zl 二 za(00(kR) (z9 —àj +z) — ài +z) 
=— cizi — zi +z + z:0v(k)lz; —&, +z) +z:(—&, +z) 


(15. 8) 
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— —c,zi — zj-- zb z;0v(k)-F z,0v(R)C—à, 十 zl) 十 zy (一 ai +z) 
——c,zi—ziczivG)-cTD-d(GvGD- DC à, +z) 
— —c,zi — z; (OvGO)) 4- D (zi 十 (一 &| 十 x1)z,) 


取 自 适应 律 
k =z} +(—å, 十 zi)z， (15. 9) 
则 
V, =—ciz? — z? + (0v(k) - DE 
两 边 积 分 可 得 


V, (t) — V, (0) --[ bo ul ye eT ohte --DEGOdr 
即 
V,(1) +| Cez? de =f Goete -- D GOde 4- V4CO 


上 式 中 ,v(k(7)) 按 N 函数 进行 设计 2 ,根据 式 (15. D , 取 
ulk) — hk! cos(k) (15. 10) 


根据 引 理 15. 1, Dll] V. Co +| Cist Dr 有 界 , 则 (cixi 十 z?) 和 | (cz 十 sd: 有 
界 , 则 由 引 理 15. 2CBarbalat 5| 3) , 当 tt 一 co 时 ,cizi 十 z: 一 0, 即 z;—0,z,—0,JÀ ifi x0, 


Ia”; 


15.3.3 仿真 实例 


针对 被 控 对 象 式 (15. 7), 取 9 二 10, 被 控 对 象 的 初始 值 为 [0. 20,0], 采 用 控制 律 
式 (15. 8) 和 自 适 应 律 式 (15.9), 取 c= 二 10,k(0)==1.0, 仿 真 结果 如 图 15-3 和 图 15-4 所 示 。 


0.3 


0.2 


e 


states response 
$ 


-0.2 


2 4 6 8 10 12 14 16 18 20 
time(s) 


图 15-3 ”状态 响应 
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30 


20 


0 2 4 6 8 10 12 14 16 18 20 
time(s) 


图 15-4 控制 输入 


仿真 程序 如 下 : 
(D) Simulink 主 程序 : chapl5 2sim. mdl。 


To Workspace3 


chap15_2plant 


S-Function To Workspace2 


Clock To Workspace 


(2) 被 控 对 象 S 函数 : chapl5 2plant. m, 


function [sys,xO,str,ts] = spacemodel(t, x,u, flag) 
switch flag, 
case 0, 
[ sys, x0, str, ts] = mdlInitializeSizes; 
case 1, 
sys = ndlDerivatives(t,x,u); 
case 3, 
sys = ndlOutputs(t,x,u); 
case {2,4,9} 
sys - []; 
otherwise 
error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0, str,ts] » mdlInitializeSizes 
sizes - simsizes; 


sizes.NumContStates  - 2; 


由 
e Oi nN O 
E 


sizes.NumDiscStates 


sizes.NumOutputs 
sizes.NumInputs 
sizes.DirFeedthrough 
sizes.NumSampleTimes 


sys = simsizes(sizes); 

x0 [0.20;0]; 

str = []; 

ts [0 0]; 

function sys = mdlDerivatives(t, x, u) 
ut-u(1); 

th2 = 10; 


sys(1) = x(1) + x(2); 

sys(2) = th2 * ut; 

function sys = mdlOutputs(t, x, u) 
sys(1) = x(1); 

sys(2) = x(2); 


(3) 控制 器 的 S 函数 ; chapl5 2ctrl. m, 


function [sys,x0, str,ts] = spacemodel(t,x,u, flag) 
switch flag, 
case 0, 
[sys, x0, str, ts] = ndlInitializeSizes; 
case 1, 
sys = ndlDerivatives(t, x,u); 
case 3, 
sys = mdlOutputs(t,x,u); 
case (2,4,9) 
sys- []; 
otherwise 
error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0, str, ts] = mdlInitializeSizes 


sizes = simsizes; 


LEE ~ 
S 


sizes. NumContStates 
sizes. NumDiscStates = 
sizes. NumOutputs = 
sizes.NumInputs = 
sizes.DirFeedthrough = 


sizes.NumSampleTimes 


Sys 7 simsizes(sizes); 


xo = [1.0]; 
str - []; 
ts - [00]; 


function sys = ndlDerivatives(t,x,u) 
xl = u(1);x2 = u(2); 
c1l- 10; 
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zl-xl; 

dz1 = x1 + x2; 

alfal = -cl*zl-zl; 
dalfal = - cl * dzl- dz1; 
z2 = x2- alfal; 


dk = z2^2 + ( - dalfal + z1) * z2; 
sys(1) = dk; 

function sys = mdlOutputs(t, x, u) 
xl =u(1);x2=u(2); 

c1 = 10; 


zl-*xli; 

dz1 = x1 + x2; 

alfal = -cl*21-zl; 
dalfal = - c1 * dz1 - dzl; 
z2 = x2 - alfal; 

k=x(1); 

vk = k^2 * cos(k); 


ut = vk * (z2 - dalfal + z1); 


sys(1) = ut; 
(4) 作 图 程序 ; chapl5 2plot. m, 


close all; 
figure(1); 


plot(t;x(:,1), r',t,x(:,2), = ;k', 'linewidEh',2); 
xlabel('time(s)');ylabel('states response'); 


figure(2); 
plot(t,ut(:,1), 'k', 'linewidth',2); 
xlabel('time(s)');ylabel('ut'); 


15.4 控制 方向 未 知 的 自 适应 反 演 控制 


15.4.1 系统 描述 
被 控 对 象 为 


zı =z: +T; 


Za =ru 
其 中 ,为 控制 输入 ,2 和 0 为 正 负 未 知 的 未 知 有 界 常数 。 
控制 目标 为 上 ce 时 ,zi 一 0,zy 一 0。 


(15. 11) 
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15.4.2 控制 律 的 设计 与 分 析 


本 节 参 考 文献 [2], 针 对 式 (15. 11) 模 型 结构 ,探讨 基于 反 演 控制 的 方法 进行 控制 器 的 设 
计 和 分 析 。 
d) 定义 z, =x, X Lyapunov 函数 


a 
Vi =g 
则 
V, EE. —zi(0ix; d- zi) 
取 
ai =2N (kz ， ki = z? (15.12) 
其 中 ,7y, 过 0。 


为 了 引入 控制 输入 u.s :一 过 一 xl W 
V, =z? =z, (0 (z: dai) +z) =z, (0,0, 十 zi) 十 0zizs 
=—zi +z +z, GONG Oz, +z) o 0,z,z; 
一 一 z1 十 2(0ONCR HT Dzi t 0,ziz; 


——g! FONG) DE 4-8, 2,; 
1 


g= ie H ON) 十 1)A 十 bz 
1 
即 


V, 4-Xut ec Lg NO.) H- DÀ, edet 
4 Yı 
根据 引 理 15. 1, 如 果 满 足 zx; 可 积 且 积分 有 界 , 则 V, C) 十 P [stie 有 界 , 从 而 z? 和 


| 有 界 , 由 引 理 15. 2(Barbalat 引 理 ), 当 £— 95H f 270, B =, 一 0, 从 而 工 , 一 0。 


(2) 证 明 22 可 积 且 积分 有 界 。 
定义 Lyapunov 函数 


l s, 
V, = uri 
其 中 ,0, 为 9, 的 估计 值 ,0, —0, —0,. 
取 N KA N Ce 0 — Ricosk, Dll a, — 2h? z,cosk,. H. 


à, —Ah, hz, cosk, 一 2h! b, z,sink, + 2k! (0 x, + x) cosk, 
=47, kı zi (2cosk; 一 k,sink,) + 2k{ z, cosk, + 2h10, x,cosk, 


d 


yi =— 47, kzi (2cosk; — k sink, ) 
d; — —2hiz;cosk; =—2N (kz (15. 13) 
p, — —2kixcosk, — —2N (kx, 
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则 a， ==—= = ,从 而 
i. —, 一 Qi = 十 由 +g: + 4,0, 


则 
V. =z; 4- 6,0, =z; (zu c Ji +g 十 ys0i) 十 (0, — 6,50, 
设计 控制 律 为 
u =N (k) ler Hp d i d 450) (15. 14) 
DU 


V, =z: (0N (ka) Cz: da d- dio +p) 二 i 十 Ys 十 301) 十 (b — 0,00, 
——zi T (0, N (k,) + ijz TT z;0; N Ge) Cg, +g 3-8 3 Pad +g) + 
24416, -- (0, — 0,8, 


取 自 适应 律 为 
0, = 2,9, (15. 15) 
k, =y i Feal Ho thh (15.16) 
其 中 :Y27705 
则 


(8, — 0,90, -- 2,40, — x, 0, 
V, —3zi-- GONG, - 02H H- 2,0, N Cb, t - 40,2 -F zs GP i) E xs, 
——zi JF (ONG) H- Dei H- z; ON G2 2-1 GJ, HF ds H 940.) 
zs -4- GN G4)-- D GI Hz, Gy-F d. 458,0) 
=— z} E ON k) DE 
即 
V. +z? < ON GU DE 
两 边 积 分 可 得 
V, +| ziar si CON (k) t DÁ;dr - VC) 


根据 引 理 15.1, V, CO +| eae 有 界 , 则 z$ 和 | sta 有 界 , 则 由 引 理 15. 2(Barbalat 引 理 ) , 
M t 一品 时 ,z: — 0, BI z, — 0.JÀ Ij m, 一 0。 

综 上 所 述 可 得 , 当 t>00 时 ,x1 一 0,7X,. 一 0。 

15.4.3 仿真 实例 


针对 被 控 对 象 式 (15. 11), 取 0, =10,0, = 二 10, 被 控 对 象 的 初始 值 为 [0. 20,0],N 函数 取 
N(k)==k*cosk, 取 7 二 30,7, 二 30, 采 用 控制 律 为 式 (15. 140 , 自 适 应 律 为 式 (15. 12) 3x15. 15) 


和 式 (15.16), 取 I (0)=1.0,k,(0)=1.0,0, (0) 王 1.0, 仿 真 结 果 如 图 15-5 和 图 15-6 所 示 。 
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states response 


4 6 8 10 
time(s) 


图 15-5 状态 响应 


e 
N 


ut 
[2] 


0 2 4 6 8 10 
time(s) 


图 15-6 控制 输入 


仿真 程序 如 下 : 
(1) Simulink 主 程序 : chap15_3sim. mdl。 
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(2) 被 控 对 象 S PR: chap15_3plant. m, 


function [sys,xO,str,ts] = spacemodel(t, x, u, flag) 
switch flag, 
case 0, 
[sys, x0, str, ts] = mdlInitializeSizes; 
case 1, 
sys = ndlDerivatives(t,x,u); 
case 3, 
sys = mdlOutputs(t, x,u); 
case (2,4,9) 
sys- []; 
otherwise 
error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0,str,ts] = mdlInitializeSizes 


Sizes - simsizes; 


sizes.NumContStates 


M 


sizes.NumDiscStates 
sizes.NumOutputs 


sizes.NumInputs 
sizes.DirFeedthrough 
sizes. NumSampleTimes 


LU 
HO mm NO NN 
M 


LU 


SyS 7 simsizes(sizes); 

x0 [1.0;0]; 

str e []; 

ts [0 0]; 

function sys = mdlDerivatives(t, x,u) 
ut - u(1); 

thl = 10;th2 - 10; 


sys(1) = thl * x(2) * x(1); 
sys(2) = th2 * ut; 

function sys = mdlOutputs(t, x, u) 
sys(1) = x(1); 

sys(2) = x(2); 


(3) 控制 器 的 SP chapl5 3ctrl. m. 


function [sys,x0, str,ts] = spacemodel(t, x,u, flag) 
switch flag, 
case 0, 
[sys, x0, str, ts] = ndlInitializeSizes; 
case 1, 
sys = ndlDerivatives(t,x,u); 
case 3, 
sys = ndlOutputs(t,x,u); 
case (2,4,9) 
sys- []; 
otherwise 
error(['Unhandled flag = ',num2str(flag)]); 
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end 
function [sys, x0, str, ts] = ndlInitializeSizes 
sizes = simsizes; 


sizes.NumContStates 


RR 


sizes.NumDiscStates  - 


S 


sizes.NumOutputs = 


EH 


sizes. NumĪnputs = 
sizes. DirFeedthrough = 


"s 


FÉÁC[|tNTPOZtu 
€ 


EE 


sizes.NumSampleTimes = 
sys = simsizes(sizes); 
x0 = [1,1,1]; 

str = []; 

ts - [00]; 

function sys = mndlDerivatives(t, x,u) 
x1l-7u(1);x27u(2); 


zl = x1; 
k1 = x(1);k2= x(2);thlp= x(3); 


Nk1 = k1^2 * cos(k1); 
alfal = 2 * Nkl * z1; 


z2 = x2 - alfal; 


Fail = -4 * z1^3 * k1 * (2 * cos(k1) -k1 * sin(k1)); 
Fai2 = -2*Nkl* zl; 
Fai3 = — 2 * Nkl * x2; 


gamal = 30; 

gama2 = 30; 

dkl = 2 * gamal * z1^2; 

dk2 = gama2 * (z2^2 + z2 * (Fail + Fai2 + Fai3 * thlp)); 
dthlp = Fai3 * z2; 

sys(1) = dkl; 

sys(2) = dk2; 

sys(3) = dthlp; 

function sys = mdlOutputs(t, x, u) 

xl = u(1);x2 = u(2); 


zl = x1; 
kl = x(1);k2= x(2);thlp= x(3); 


Nkl = k1^2 * cos(k1); 
alfal = 2 * Nkl * z1; 


z2 = x2— alfal; 
Fail = — 4 * kl * z1^3 * (2*cos(kl)- kl * sin(k1)); 


Fai2 = -— 2 * Nk1 * z1; 
Fai3 = -2*Nkl * x2; 
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[1] 


[2] 


[3] 


Nk2 = k2^2 * cos(k2); 
ut = Nk2 * (z2 + Fail + Fai2 + Fai3 * thlp); 


sys(1) = ut; 
(4) 作 图 程序 : chapl5 3plot. m, 


close all; 


figure(1); 
plot(t,x(:,1), r',t,x(:,2), =  k', 'linewidth', 2) ; 
xlabel('time(s)');ylabel('states response'); 


figure(2); 
plot(t,ut(:,1), 'k', 'linewidth',2); 
xlabel('time(s)');ylabel('ut'); 
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多 智能 体系 统一 致 性 控制 的 
设计 与 分 析 


CHAPTER 16 


16.1 多 智能 体系 统 介绍 


多 智能 体系 统 (Multi-Agent System,MAS) 是 当今 人 工 智能 中 的 前 沿 学 科 ,是 分 布 式 人 
工 智能 研究 的 一 个 重要 分 支 , 其 目标 是 将 大 的 复杂 系统 ( 软 硬 件 系统 ) 建造 成 小 的 、 彼 此 相 
互通 信 及 协调 的 、 易 于 管理 的 系统 。 多 智能 体 的 研究 涉及 智能 体 的 知识 目标、 技能、 规划 以 
及 如 何 使 智能 体 协 调 行动 解决 问题 等 。 

1989 年 举行 的 第 一 届 国 际 多 智能 体 欧 洲 学 术 会 议 , 标 志 着 该 技术 受到 了 研究 者 的 广泛 
重视 。1993 年 首次 召开 了 智能 体形 式 化 模型 国际 会 议 ,1994 年 又 召开 了 第 一 届 智 能 体 理 
E .体系 结构 和 语言 国际 会 议 , 这 表明 多 智能 体 技术 获得 了 越 来 越 多 的 关注 。 


16.1.1 多 智能 体系 统 特点 


多 智能 体系 统 在 表达 实际 系统 时 ,通过 各 智能 体 间 的 通信 .合作 、 互 解 . 协 调 、 调 度 、 管 理 
及 控制 来 表达 系统 的 结构 .功能 及 行为 特性 。 多 智能 体系 统 具 有 自主 性 、 分 布 性 ,协调 性 ,并 
具有 自 组 织 能 力 ,学 习 能 力 和 推理 能 力 。 采 用 多 智能 体系 统 解决 实际 应 用 问题 ,具有 很 强 的 
鲁 棒 性 和 可 靠 性 ,并 具有 较 高 的 问题 求解 效率 。 

多 智能 体系 统 有 以 下 特点 : 

(1) 每 个 智能 体 具 有 独立 性 和 自主 性 ,能 够 解决 给 定 的 子 问题 ,自主 地 推理 和 规划 并 选 
择 适 当 的 策略 。 

(2) 多 智能 体系 统 具 有 良好 的 模块 性 .易于 扩展 性 和 设计 灵活 性 ,克服 了 建设 一 个 庞大 
的 系统 所 造成 的 管理 和 扩展 的 困难 ,能 有 效 地 降低 系统 成 本 。 

(3) 各 智能 体 通 过 互相 协调 去 解决 大 规模 的 复杂 问题 ,并 可 将 各 子 系统 的 信息 集成 在 
一 起 ,完成 复杂 系统 的 集成 ,能 有 效 地 提高 问题 求解 的 能 力 。 


16.1.2 多 智能 体系 统 应 用 领域 


1) 智能 机 器 人 

在 智能 机 器 人 中 ,各 机 器 人 通过 共享 信息 、 相 互 协调 ,来 有 效 地 完成 总 体 任 务 。 利 用 多 
智能 体系 统 , 将 每 个 机 器 人 作为 一 个 智能 体 ,建立 多 智能 体 机 器 人 协调 系统 ,可 实现 多 个 机 
器 人 间 的 相互 协调 与 合作 ,完成 复杂 的 并 行 作 业 任 务 。 
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2) 交通 控制 

由 于 交通 控制 拓扑 结构 的 分 布 式 特性 ,多 智能 体 技术 很 适合 应 用 于 交通 控制 ,尤其 对 于 
具有 剧烈 变化 的 交通 情况 ， Rd e 

3) 柔性 制造 

在 制造 系统 中 , 各 加 工 单元 可 看 作 智 能 体 ， 从 而 使 加 工 过 程 构成 一 个 半 自 治 的 多 智 
能 体制 造 系统 ,完成 单元 内 加 工 任务 的 监督 和 控制 。 多 智能 体 技术 可 用 于 制造 系统 的 
调度 。 

4) 分 布 式 预测 ,监控 及 诊断 

利用 多 智能 体 的 联合 意图 机 制 可 实现 联合 行动 ,从 而 实现 分 布 式 预测 与 监控 。 

5) 分 布 式 计 算 

用 多 智能 体 技术 建立 分 布 式 计 算 环 境 的 基本 目标 是 建立 各 种 客户 / 服务 器 应 用 ,其 核 
心 是 基于 智能 体 的 服务 请 求 代理 机 制 , 它 分 为 两 部 分 : 由 客户 应 用 和 服务 请 求 智能 体 组 成 
的 客户 环境 和 由 一 组 服务 智能 体 组 成 的 服务 环境 。 

6) 产品 设计 

产品 设计 问题 涉及 多 目标 的 约束 求解 和 设计 过 程 的 协调 。 利 用 多 智能 体系 统 的 并 行 处 
理 技术 将 不 同 的 任务 分 解 ,分 别 分 布 在 不 同 的 智能 体 上 ,可 以 降低 设计 费用 ,提高 设计 的 
速度 。 

7) 控制 系统 设计 

利用 多 智能 体 技术 可 建立 一 个 多 智能 体 控制 系统 框架 ,包括 三 层 : 最 底层 为 控制 层 , 具 
有 实时 控制 能 力 ; 中 间 层 为 管理 层 ; 最 上 层 为 多 智能 体 协 调 与 通信 层 。 该 框架 可 解决 实际 
系统 的 控制 问题 ,框架 内 每 个 智能 体 负责 各 自 的 控制 任务 。 


16.1.3 多 智能 体系 统 在 机 器 人 控制 中 的 应 用 


目前 , 美国 ,英国 、 法 国 和 澳大利亚 等 国家 都 在 从 事 基于 多 智能 体系 统 的 机 器 人 方向 研 
究 , 我 国 也 将 这 方面 的 研究 列 入 国防 科 工 委 “ 八 五 ” 预 研 项 目 。 在 智能 机 器 人 中 , 信息 集成 
和 协调 是 一 项 关键 性 技术 , 它 直 接 关系 到 机 器 人 的 性 能 和 智能 化 程度 。 一 个 智能 机 器 人 应 
包括 多 种 信息 处 理子 系统 ,如 二 维 或 三 维 视觉 处 理 、 信 息 融 合 、 规 划 决 策 以 及 自动 驾驶 等 。 
各 子 系统 是 相互 依赖 、 互 为 条 件 的 , 它们 需要 共享 信息 、 相 互 协调 , 才能 有 效 地 完成 总 体 任 
务 , 其 目标 是 用 来 结合 ,协调 、 集 成 智能 机 器 人 系统 的 各 种 关键 技术 及 功能 子 系统 ,使 之 成 为 
一 个 整体 以 执行 各 种 自主 任务 。 文 献 [3] 中 ,作者 设计 了 单个 机 器 人 多 智能 体系 统 , 采 用 实 
时 黑板 智能 体 作为 框架 的 核心 , 实现 了 分 布 式 黑板 结构 ,并 采用 分 布 式 问题 求解 、 实 时 知 
识 库 及 实时 推理 技术 , 以 提高 机 器 人 的 实时 响应 速度 ,该 机 器 人 已 成 功 地 应 用 于 自主 式 水 
下 车 辆 的 声 纳 信 号 解释 。 在 多 机 器 人 系统 中 ， 当 多 个 机 器 人 同时 从 事 同 一 项 或 多 项 工作 
时 , 很 容易 出 现 冲 突 ” 。 利 用 多 智能 体 技术 , 将 每 个 机 器 人 作为 一 个 智能 体 , 建立 多 智能 
体 机 器 人 协调 系统 ,可 实现 多 个 机 器 人 的 相互 协调 与 合作 , 完成 复杂 的 并 行 作 业 任 务 。 文 
献 L5] 中 ,作者 提出 了 一 种 用 于 火星 编队 保持 和 运动 轨迹 跟踪 的 多 智能 体系 统一 致 性 控制 器 
的 设计 和 分 析 方 法 。 文 献 L6] 描 述 了 一 个 移动 机 器 人 协作 的 多 智能 体系 统 结构 ,并 设计 了 模 
糊 逻 辑 的 控制 方法 。 文 献 [7] 讨 论 了 多 机 器 人 控制 系统 中 的 多 智能 体系 统 设计 及 人 机 交互 
问题 。 
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16.1.4 ”多 智能 体 控制 展望 


作为 分 布 式 人 工 智能 的 重要 组 成 部 分 , 多 智能 体 控制 技术 的 理论 与 应 用 研究 刚刚 起 
步 , 还 有 不 少 问题 有 待 解 决 ,主要 集中 在 以 下 几 方 面 : 多 智能 体 控制 系统 的 实时 性 、 和 鲁 棒 
性 、 自 适应 性 和 稳定 性 ; 网 络 环境 下 的 多 智能 体 控制 系统 的 约束 和 优化 问题 ; 多 智能 体 控 
制 系统 的 协调 问题 ; 逻辑 符号 与 数学 计算 相 结 合 的 多 智能 体系 统 。 

多 智能 体 技术 是 目前 人 工 智能 领域 中 重要 的 研究 方向 之 一 。 随 着 网 络 技术 的 发 展 , 多 
智能 体 技术 的 应 用 领域 不 断 扩大 , 现 已 面向 社会 领域 的 各 个 方面 。 如 何 将 多 智能 体 技术 应 
用 于 机 器 人 控制 领域 已 成 为 当前 最 为 迫切 的 任务 之 一 。 相 信 基 于 多 智能 体 技 术 的 分 布 式 智 
能 控制 将 成 为 智能 控制 的 一 个 重要 的 研究 方向 ,多 智能 体 技术 将 为 复杂 系统 的 综合 集成 提 
供 一 条 新 的 途径 。 


16.2 一 阶 多 智能 体系 统 的 一 致 性 控制 


16.2.1 系统 描述 


考虑 如 下 一 阶 多 智能 体系 统 
r; =u; 4 d, (16. 1) 

其 中 ,i 二 1,2,…,n,u; 为 第 i 个 智能 体 的 控制 输入 ,d, 为 加 在 控制 输入 上 的 扰动 ， 
I d; || 入 D,。 

在 该 一 阶 动态 模型 中 ,r, 是 第 i 个 跟随 者 智能 体 的 位 置 ,u; 是 控制 输入 。 假 设 领导 者 
智能 体 为 0 号 智能 体 ,其 位 置 和 速度 分 别 为 r。 和 ro, 且 rs ll ry, ,定义 rj 二 ri 一 ro。 

iX r—[rn c 7,] ,假设 虚拟 领导 者 智能 体 的 速度 是 时 变 的 ,跟随 者 之 间 的 图 是 
无 向 连通 图 , 且 至 少 有 一 个 跟随 者 能 获取 领导 者 的 位 置信 息 。 控 制 目标 是 在 测量 速度 未 知 
时 ,设计 控制 器 ,使 得 所 有 追随 者 智能 体 都 以 通过 局 部 交互 跟踪 虚拟 领导 智能 体 , 即 19 oo 
B .r-—0. 


16.2.2 控制 器 的 设计 
参考 文献 [8] 的 设计 思路 ,进一步 考虑 克服 控制 输入 扰动 的 问题 ,设计 和 鲁 棒 控 制 器 , 表 
示 为 
u, — 08 (r; EEE TEN h Sas (r; —r;2) (16. 2) 
其 中 ,a.p, 都 为 正常 数 ,8 之 y +D, ,sgn 是 符号 函数 。 
EALS, = DJa, (r, 一 盖 ), 则 控制 器 可 以 表示 为 


u; — —a$,; — B;sgnS; 
首先 将 控制 器 式 (16. 2) 代 入 模型 式 (16. 1) ,可 得 
F, — F, —^, =u; +d; —F, =—aS; — B,sgnS, +d; 一 Fu 


HTr,—r,-—r,.r; r,;—r, r , iij 
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S$,; = b" (r; —r;)— 274g (r; —r;)--ag(r;—r?,)-— bj? (r; —;)-- as; 
j-l j=1 


定义 r= [r，7。 ri] ON 
=a D agri — Dagr + uF) egal (Das, -De rj 二 air;)+d, —F, 
j=1 7 一 1 
(16. 3) 
考虑 取 ? 一 3 时 ,将 S, 展开 可 得 
S= Mayr, = Dasr, Tar; = lan Faia TQ jr — CHA Fair: 十 aisrs) tant; 
jet j=1 
即 
a io 
S — (ag "di deaf, (gf, -Faas Hiara) F a 29 r 
a 30 
-—Lr + diag (a 10.3 T TTD E F =Mr 
HKoB.S—1S;).1—1.2,-,n, M—L -diag(a, sas) ao) 江 为 Laplacian 和 矩阵 ,该 矩阵 主 
要 应 用 于 图 论 中 ,作为 一 个 图 的 矩阵 表示 , 工 为 半 对 称 正 定 和 矩阵,M 为 对 称 正 定 和 矩阵 ， 
sgn(Mr) 二 sgn(r)。 定 义 L 二 {1;}€ER"”" ,考虑 到 ;= 一 ) 时 ,a 二 0, 则 可 取 
= Maj, ly =—ay(i)) 


j=1i) 


则 式 (16. 3) 可 写 为 
F — —aS 一 psgnS +I (d; — to) 
Bp 
r — —aMr — fisgn (Mr) + I (d; — i,) (16. 4) 


16.2.3 稳定 性 分 析 


定义 Lyapunov 函数 


则 
V — rF* Mr — F'M (— aMr — fisgnMr — I (d, —54)) 
<—ar Mr — Bl Mr ||; + |d; —7r,| ll Mr I 
«— ar Mr — 8| Mr ll, y, || Mr ll; €—ar'M'r <0 
则 ;一 co 时 ,F->0。 


16.2.4 仿真 实例 


智能 体 i 位 置 表示 为 疡 = [Da ry] ,ri 和 rs 分别 为 智能 体 i 在 x 方向 和 y 方向 的 位 
置 , 若 智能 体 ; 和 智能 体 7 相连 通 , 则 a; 二 1, 否 则 a 
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考虑 由 1 个 领导 者 智能 体 和 3 个 跟随 者 智能 体 构 成 的 多 智能 体系 统 , 系 统 结构 如 图 16-1 
BR .i—1,.2,3,j —1,2,3, 

图 16-1 P.L 为 领导 者 智能 体 ,F1、F2、F3 为 跟随 者 智能 体 。 

根据 图 16-1 EE X..a,,—1.a2 —0.a5, —0. n] 18 Laplacian É F3 


ABE L 表达 式 为 ee SN 


Fl — F2 
L-íi)-|-1 2 —1 
—1 =] 2 图 16-1 多 智能 体系 统 结构 


M =L +diag 0 0) 
根据 MATLAB 命令 ,采用 仿真 程序 chapl6_1L. m, 可 得 工 特征 值 为 : esig(L)=[0 3 3]. 
M 特征 值 为 ; eig(MO —[0.2679 3.0000 3.7321] ,满足 设计 要 求 。 
W ru(t)= [cost sint] ,wd, 王 3sint, 采 用 控制 器 式 (16. 2) , 取 a — 1, y, — 1. 414 8 HS 
B, Y, D, "IK 8—3.5, 
为 了 防止 拌 振 de d aeh RAUA PR sate 代替 符号 函数 sgna ,设计 如 下 


1 a FA 
i le (Ss k =1A 
=l; d eA 
其 中 ,A 为 边界 层 。 
采用 饱和 函数 控制 实质 为 : 在 边界 层 之 外 ,采用 切换 控制 ,使 系统 状态 快速 趋 于 收敛 
值 ,在 边界 层 之 内 ,采用 反馈 控制 ,以 降低 快速 切换 时 产生 的 拌 振 。 
控制 律 中 ,采用 饱和 函数 代替 符号 函数 , 取 A=0.01, 所 有 智能 体 的 一 致 性 控制 跟踪 如 
图 16-2 所 示 ,x 方向 和 >y 方向 的 位 置 跟踪 误差 如 图 16-3 所 示 ,z 方 向 和 y 方向 的 控制 输入 
如 图 16-4 所 示 。 


——— leader agent 
first agent 
second agent 

= third agent 


图 16-2 3 个 智能 体 的 跟踪 
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error between rix and rOx 


time(s) 


2 
© 
S 
2 
E 
一 | 
9 
E 
2 
E 
v 
time(s) 
图 16-3 3 个 智能 体 在 x 和 y 方向 的 响应 
500 
E 
2 0 
x 
| 
-500 
0 2 4 6 8 10 
time (s) 
50 
5 
号 o E 
x 
As Ml 
A EMEN MEM 
E 2 4 6 8 10 
time (s) 
200 
& 
2 0 
x 
e 
2 


2006 2 4 6 8 10 


time (s) 


16-4 3 个 智能 体 在 x y 方向 的 控制 输入 
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仿真 程序 如 下 : 
(OD 主 程序 : chap16_1sim. mdl, 


hap16_tinput 


chap!6 icti3 
7 


chap16 1plant3 
S-Function1 


(2) 领导 者 智能 体 世子 程序 : chapl6 linput. m. 


function [sys, x0, str,ts] = func(t,x,u,flag) 
Switch flag, 
case 0, 

[sys, x0, str, ts] = ndlInitializeSizes; 
case 1, 

sys = ndlDerivatives(t,x,u); 
case 3, 

sys = ndlOutputs(t,x,u); 
case {2,4,9} 

sys= []; 
otherwise 

error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0, str, ts] = ndlInitializeSizes 


Sizes - simsizes; 


sizes.NumContStates 


Li 
[| HÍ|!otoo 
Ws 


sizes.NumDiscStates 
sizes.NumOutputs 


sizes.NumInputs 
sizes.DirFeedthrough 
sizes.NumSampleTimes 


Sys = sinmsizes(sizes); 


x0 -[]; 
str = [i 
ts = [00]; " 


function sys = mdlOutputs(t, x, u) 
r0 = [cos(t),sin(t)]'; 


sys(1) = r0(1); 
sys(2) = r0(2); 
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(3) 跟随 者 智能 体 Fl 控制 器 子 程序 : chapl6 lctrll. m, 


function [sys, x0, str,ts] = func(t,x,u, flag) 
switch flag, 
case 0, i 
[ sys, x0, str, ts] = mdlInitializeSizes; 
case 1, 
sys = mdlDerivatives(t, x,u); 
case 3, 
sys = mdlOutputs(t, x,u); 
case {2,4,9} 
sys=[]; 
otherwise 
error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0, str, ts] = mdlInitializeSizes 


sizes = simsizes; 


E^ É © VOO 
` 


sizes. NumContStates 
sizes. NumDiscStates = 
sizes. NumOutputs = 
sizes. NumInputs = 
sizes.DirFeedthrough = 
sizes. NumSampleTimes = 


sys = simsizes(sizes); 


x0 -[]; 
str [ly 
ts - [00]; 


function sys = mdlOutputs( t, x, u) 
alfa = 1. 0;beta = 3.5; 

r0 - [u(1) u(2)]'; 

rl- [u(3) u(4)]'; 

r2- [u(5) u(6)]'; 

r3 - [u(7) u(8)]'; 


a01 = 1;a21 = 1;a31 = 1; 
suml = a01 * (r1 ~ r0) + a21 * (rl-r2)*a31* (r1- r3); 


% Saturated function 
delta = 0. 01; 
kk = 1/delta; 
if abs(sum1)> delta 
satl = sign(suml); 
else 
sat1 = kk * suml; 


end 
%ul = -alfa * sumi ~ beta * sign(sum1); 
ul = -alfa * suml - beta * satl; 


sys(1) = u1(1); % ulx; 
sys(2) = u1(2); % uly; 
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(4) 跟随 者 智能 体 F1 被 控 对 象 子 程序 : chap16_1plantl. m, 


function [ sys, x0, str, ts] = Model(t,x,u, flag) 
switch flag, 
case 0, 

[ sys, x0, str, ts] = ndlInitializeSizes; 
case 1, 

sys = ndlDerivatives(t,x,u); 
case 3, 

sys = ndlOutputs(t, x,u); 
case (2, 4, 9 ) 

sys = []; 
otherwise 

error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0, str, ts] = mdlInitializeSizes 


sizes = simsizes; 


sizes.NumContStates 


sizes.NumDiscStates 
sizes.NumOutputs 


sizes.NumInputs 
sizes.DirFeedthrough 
sizes.NumSampleTimes 


LU 
e 0t NO IN 
EM 


Sys 7 simsizes(sizes); 

x0=[0.5 0.5]; 

str=[]; 

tss | =1 0]; 

function sys = mdlDerivatives(t, x,u) 
ulx- u(1); 

uly= u(2); 


sys(1) = ulx; 

sys(2) = uly; 

function sys = mdlOutputs(t, x, u) 
rl= [x(1) x(2)]; 

sys(1) = r1(1); 

sys(2) = r1(2); 


(5) 作 图 子 程序 : chapl6 lplot. m, 


close all; 

figure(1); 

plot(r0(:,1),r0(:,2), 'r', 'linewidth',2); 
xlabel( 'r0x');ylabel('r0y'); 


hold on; 
plot(rl(:,1),r1(:,2), 'g', 'linewidth',2); 
hold on; i 
plot(r2(:,1),r2(:,2), 'b', 'linewidth',2); 
hold on; 


plot(r3(:,1),r3(:,2), 'k', 'linewidth',2); 
legend( leader agent', 'first agent', 'second agent', 'third agent!) ; 
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figure(2); 

subplot(211); 

plot(t,rl1(:,1)- r0(:,1), 'g',t,x2(:,1) = Ele 'b', t6,23(:,1) — €0( :, 1) , ky 'Tánewidth*, 2); 
xlabel('time(s)');ylabel('error between rix and r0Ox'); 

legend('rix- r0x', 'r2x- rOx', 'r3x - r0x'); 

subplot(212); 

plót(t,rl(:,2) —r0(:,2), 'g',t,r2(:,2) = z0(:,2) ; b", £j 2£3(:,2) 9 x0(:, 2); 'k*, "linewidth*,2; 
xlabel('time(s)'); ylabel('error between riy and r0y'); 

legend('rly- rOy','r2y- rOy', r3y- r0y'); 


figure(3); 

subplot(311); 

plot(t,ul(:,1),'b',t,u1(:,2), E", "M«inewidth',2); 
xlabel('time(s)');ylabel('ulx,uly'); 
legend('first agent'); 

subplot(312); 

plot(t,u2(:,1), 'b',t,u2(:,2), 'r', 'linewidth',2); 
xlabel('time(s)'); ylabel('u2x,u2y'); 

legend( second agent"); 

subplot(313); 

plot(t,u3(:, 1), 'b', t,u3( :,2), 'z", '1linewidth',2); 
xlabel('time(s)');ylabel('u3x, u3y'); 

legend( third agent'); 


(6) Laplacian 4# RE L 和 和 矩阵 M 的 测试 : chapl6 1L. m, 


close all; 
n= 3; 


al2=1;al3=1;a23=1; 


L11 = a12 + a13; 
L22 = a12 + a13; 
L33 = a12 + a13; 


112 = —-a12; 
L21 = L12; 


L13* —a13; 
131 = L13; 


L23 = - a23; 
L32 = L23; 


L= [L11 L12 L13; 
L21 L22 L23; 
L31 L32 L33]; 


%L=[2 -1 -1; 
$ -1 2-1 
$ -1-1 2] 


al0-71;a20-0;a30-0; 
A= [a10 0 0;0 a20 0;0 0 a30]; 
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M-LtA; 


eig L= eig(L) 
max(eig L) 


eig M- eig(M) 


16.3 非 线性 多 智能 体系 统一 致 性 控制 


16.3.1 问题 描述 


考虑 一 组 具有 如 下 一 阶 时 变动 态 非 线性 智能 体系 统 "” 
E, ftx) Hg sx: Ju; d di) (16. 5) 
其 中 必 一 1,2,…ziER 表 示 第 ;个 智能 体 的 状态 ,ww ER 表示 第 i 个 智能 体 的 控制 输 
入 ,函数 f; 表示 第 i 个 智能 体 的 非 线 性 动态 ,函数 g, 表示 与 状态 相关 的 控制 方程 ,函数 d, 
表示 外 部 干扰 ,1d; GI D;. 
假设 函数 f; 和 g; 均 未 知 ,满足 
| Fin |S prts quer) £g, (16. 6) 
其 中 ,pi (t,x;) 和 go 分别 是 已 知 的 非 负 函数 和 正常 数 。 
考虑 一 组 非 线性 系统 的 通信 拓扑 为 无 向 , 即 若 智能 体 ; 和 智能 体 7 可 以 进行 双向 通信 ， 
则 有 a; —aj 1,189] aj =a; —0, HH a; —0, W A—[a;; ]€ R""", 


16.3.2 控制 器 的 设计 与 分 析 
针对 式 (16. 5) 描 述 的 一 组 智能 体系 统 , 设 计 如 下 控制 器 


i: = 一 二 senz， (px) +D: + | ME Gi 201) (16. 7) 
0 j=1 
其 中 ,7 二 0。 
针对 第 i 个 智能 体 , 构 造 如 下 Lyapunov 函数 
V, 一 也 
则 
V, 一 工 之 ， 


=x; (f: tszi) +t gi Gru; 4d; Q2) 
S| rip; Gs) (F g;Gsxxu; T xD; | 


zt. » - 
=| xip: (tx) EEE v sgnz, (p G 1; +D; + | M; (x; z;)]| | 7 ) H| zxD, | 
&o j=1 


S| Epi ltr) |—| ws | TER I+D: + | Dasz,—z)]|+n) 十 | x;D; | 
je 


=- z, | (|Ð las  201| y.) «0 
j=l 
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当 且 仅 当 zx; 二 0 Bf, V; 一 0, 当 t 习 oo 时 ,x 一 0, 且 收敛 速度 取决 于 第 i 个 智能 体 与 相通 
信 的 智能 体 之 间 的 间隔 值 。 
16.3.3 仿真 实例 


考虑 具有 如 图 16-5 所 示 通 信 拓 扑 的 4 个 多 智能 体 , 取 4 个 多 智能 体 初 始 状态 为 x (00 — 
一 3,zs(0) 一 一 5,zs(0) 一 5,zi(0) 王 1。 图 中 相 邻 2 个 智能 体 之 间 均 为 无 向 图 , 即 


Q 1 Q i 
"m 0 1 0 
0 1 0 1 
1 0 1 0 
根据 式 (16. 5) ,考虑 每 个 智能 体 的 非 线 性 动态 为 图 16-5 通信 拓扑 图 
Ti =f tto Fg itr du i21,2,3,4 
其 中 ， 
f Gx;)-——0. lx; 4-0. 2x! sinCO. 12) <| 0.1z, |+0. 2x? 
g, (C x,) —0.1cosr, +0.2 2 0.2 
d;(t) —0. 1cosCO. 12) 
则 可 取 


pi(tsz;)-—|0.1x; |--0.2z2, go 一 0.2， D,-0.10, 太一 0.01 
根据 式 (16.7) ,控制 律 为 
u; =— 5 senz: (| 0. 1x; | 十 0. 2x; +0.10 + | RERO —u; 13-0. 01) 
为 了 防止 拌 振 ,控制 器 中 采用 饱和 函数 代替 符号 函数 ,具体 算法 同 16. 2 35,56 A — 0. 001, 
仿真 结果 如 图 16-6 和 图 16-7 Bron. 


x1,x2,x3,x4 


time(s) 
16-6 ”系统 状态 x, KAAR 
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jj, m —— í— - 
80! i i 一 一 一 firstagent |i 
o cesves ESS 1 7 second agent |: 
| ! ! ——ahirdagent | 
A nn E ER 了 fourth agent. |} 
e en crc d o TE | 
' ! | 
a a RR. "SDE NN 1 
E 1 | r 1 
3 | i i i i 
ci 上 1 L L 
= 1 I 1 L L 
3 .Se IPIE — 
i i i i 1 
Ü 上 t L Uu 
qosuecar ai qd a pv pn Ee EN emi dri a7 
1 L ' LU | 
i i i i i 
personos Ne Estee SB i 
i i i i i 
2 4 6 8 10 
time(s) 
图 16-7 控制 输入 
Simulink 仿真 程序 如 下 : 


CD 主 程序 : chap16_2sim. mdl。 


S-Functiont 


(2) 智能 体 控 制 器 子 程序 : chapl6 2ctrl. m, 


function [sys,x0, str,ts] = Ma_ctrl(t,x,u, flag) 
switch flag, 
case 0, 
[sys, x0, str, ts] = mdlInitial izeSizes; 
case 3, 
sys = mdlOutputs(t, x,u); 
case (1,2,4,9) 
sys-[]; 
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otherwise 

error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys,x0, str,ts] = mdllInitializeSizes 
sizes = simsizes; i 


sizes. NumContStates 


sizes. NumDiscStates 
sizes.NumOutputs 


sizes.NumInputs 


Ll 
O re 5» oo 
T 


sizes.DirFeedthrough 
Sizes.NumSampleTimes = 
sys = simsizes(sizes); 

x0 =[]; 

str - []; 

ts = []; 

function sys = mdlOutputs(t, x, u) 
xl=u(1); 

x2 = u(2); 

x3 = u(3); 

x4 7 u(4); 

X= [xl x2 x3 x4]; 


al2=1;al3= 0;a14= 1; 
a21 = 1;a23 = 1;a24 = 0; 
a31 = 0;a32 = 1;a34 = 1; 
a41 = 1;a42 = 0;a43 = 1; 
all = 0;a22 = 0;a33 = 0;a44 = 0; 


g0 = 0. 20; 


% Saturated function 
delta = 0. 001; 
kk = 1/delta; 


for i=1:4 
if abs(X(i))> delta 
sat x(i)= sign(X(i)); 
else 
sat x(i)= kk * X(i); 
end 
end 


suml- a12 * (xl- x2) + a13 * (x1 - x3) * al4 * (x1- x4); 
sum2 = a21 * (x2- x1) * a23 * (x2- x3) * a24 * (x2- x4); 
sum3 = a31 * (x3- x1) + a32 * (x3- x2) + a34 * (x3- x4); 
sum4 = a41 * (x1 - x2) + a42 * (x4 - x2) + a43 * (x4 - x3); 


xite - 0.01; 

ul = -1/g0* sat x(1) * (abs(0.1* x1) * 0.2* x12 * 0.1 * abs(suml) * xite); 
u2- —1/g0* sat x(2) * (abs(0.1* x2) * 0.2* x2^2* 0.1 * abs(sum2) + xite); 
u3 = —1/g0* sat x(3) * (abs(0.1* x3) * 0.2* x3^2* 0.1 + abs(sum3) + xite); 


第 16 章 “多 智能 体系 统一 致 性 控制 的 设计 与 分 析 | 417 


u4= — 1/g0 * sat x(4) * (abs(0.1* x4) * 0.2* x4^2 * 0.1 abs(sum4) + xite); 


sys(1) = ul; 
sys(2) = u2; 
sys(3) = u3; 
sys(4) = u4; 


(3) 智能 体 被 控 对 象 子 程序 : chapl6 2plant. m, 


function [sys, x0, str, ts] = Ma plant(t, x, u, flag) 
switch flag, 
case 0, 
[sys, x0, str, ts] = ndlInitializeSizes; 
case 1, 
sys = ndlDerivatives(t,x,u); 
case 3, 
sys = mdlOutputs(t,x,u); 
case (2, 4, 9) 
sys = []; 
otherwise 
error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0, str, ts] = mdlInitializeSizes 


sizes - simsizes; 


sizes.NumContStates = 4; 
sizes.NumDiscStates = 0; 
sizes.NumOutputs = 4; 
sizes.NumInputs = 4; 
sizes.DirFeedthrough = 0; 
sizes.NumSampleTimes = 0; 
Sys = simsizes(sizes); 

x0 -[-3 -551]; 

str -[] 

ts -[); 

function sys = ndlDerivatives(t, x,u) 
ul-u(1); 

u2 = u(2); 

u3 = u(3); 

ud = u(4); 

x17 x(1); 

x2 = x(2); 

x3 - x(3); 

x4 = x(4); 


dt-0.1*cos(0.1* t); 

dxl= -0.1*x140.2* (x1^2) * sin(0.1* t) * (0.1* (cos(x1).^2) *0.2) xul * dt; 
dx2= —0.1* x2*0.2* (x2^2) * sin(0.1* t) + (0.1* (cos(x2).^2) * 0.2) * u2 + dt; 
dx3= —-0.1* x3 0.2* (x3^2) x sin(0.1* t) + (0.1* (cos(x3).^2) * 0.2) x u3 + dt; 
dx4- —0.1*x4-*0.2* (x42) * sin(0.1*t) + (0.1* (cos(x4).^2) 0.2) *u4 * dt; 
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sys(1) = dx1; 
sys(2) = dx2; 
sys(3) = dx3; 


sys(4) = dx4; 

function sys = mdlOutputs(t,x,u) 
sys(1) - x(1); 

sys(2) = x(2); 

sys(3) = x(3); 

sys(4) - x(4); 


(4) 作 图 子 程序 : chapl6 2plot. m. 


close all; 

figure(1); 

grid on; 
xlabel('time(s)');ylabel('xl,x2,x3,x4'); 
hold on; 

plot(t,x(:,1), 'r', 'linewidth',2); 

hold on; 

plot(t,x(:,2), 'g', 'linewidth',2); 

hold on; 

plot(t,x(:,3), 'b', 'linewidth',2); 

hold on; 

plot(t,x(:,4), 'k', 'linewidth',2); 
legend('first agent', 'second agent', 'third agent', 'fourth agent'); 


figure(2); 

grid on; 
xlabel('time(s)');ylabel('ul,u2,u3,u4'); 
hold on; 

plot(t,ut(:,1), 'r', 'linewidth',2); 

hold on; 

plot(t,ut(:,2), 'g', 'linewidth',2); 

hold on; 

plot(t,ut(:,3), 'b', 'linewidth',2); 

hold on; 

plot(t,ut(:,4), 'k', 'linewidth',2); 
legend('first agent', 'second agent', 'third agent', 'fourth agent') ; 


16.4 线性 多 智能 体系 统一 致 性 控制 


16.4.1 系统 描述 
针对 如 下 二 阶 线性 多 智能 体系 统 : 


Ti STi 
Ze =u; td; 
yi TTia 


#p,l|d, | Sd isax o 


(16. 8) 
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针对 智能 体 i 和 智能 体 j,(j ,i) EE 表示 智能 体 i 可 以 获得 智能 体 7 的 信息 ,智能 体 i 
的 相 邻 集合 表示 为 A,;=={j|1(j ,i) EE)。 

智能 体 i 与 智能 体 ) 之 间 连 接 的 标记 取 a, ,a, =1 时 表示 智能 体 i 与 智能 体 j 之 间 有 
通信 ,否则 a; ==0, 且 有 (a; 二 0,A=[a; ] ER“, 


N 
4E XB —diaglE e, En) E = »a; , 定义 Laplacian 矩阵 为 
| 


L—H-—A 
一 致 性 指令 为 y, ,智能 体 i 与 指令 yo 之 间 连 接 的 标记 取 jy; ,pi 二 1 时 表示 智能 体 ; 可 
以 获得 指令 yo 信息 ,否则 取 jy 二 0, 取 
p — diagly, se rp) 
控制 目标 为 : t—o0Hlf tai yos Ta 36» 


16.4.2 控制 律 的 设计 
智能 体 i BUR ER Dx 25 J €, — y, — yo E X 


£; —€, FE; 

z; =p: yi — y) + Š, y: — (16. 9) 
JEA; 

z, =z; 十 z, 


由 于 y; 一 yj 二 ;一 ej ,根据 a; 的 定义 ,有 
zi =p; + Japle — 65) — (qu; + Be; — D ape, 
p p 


由 于 Mass, — Ae, . Jill 


z =(u +E )e—Ae —(L +p )e (16. 10) 
根据 控制 目标 ,设计 Lyapunov 函数 为 
V=7 € rae (16.11) 
则 
V=ē"(L +p) é =z" È 
由 于 


Š; 一 8， +é, =y; —yo +y; —yo = Fd; xo — pi C yo 十 yo ) 十 (HA D DG, 十 yo ) 
—u; t xa —pu;Cyo X) 9 D, . 
K'P.D,—d,-- G;—D Gs X0 D imr dius suplyo-- Yol HE bf i 可 以 获得 指令 
yo 信息 时 ,六 — 1.5 BI D,—d;. 
则 
N 
V — z; (u; 4-55 — pu; ys 十 yo) + D;) 
i=l 
设计 控制 律 为 
u; 一 一 ci 二 一 了 十 Ai(Cyo Hyo) — g;sgnz, (16. 12) 
Kho Diu 
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则 

= 一 ET. «o 
M 1790 时 ,=, 一 0, 根 据 式 (16. Vus m Rif ed 且 E; 一 0。 
16.4.3 仿真 实例 


考虑 如 图 16-8 所 示 的 多 智能 体系 统 拓 扑 结构 2 ,只 有 第 二 个 智能 体 与 指令 yo 相连 ， 
1s 二 1,yo 二 sint。 针 对 多 智能 体系 统 式 (16. 8) d; =3sint i —1,2,3,4,24 i —2 时 ,Di 一 


d imax FT RC 3,238; M i=1,3,4 R$, D imax =d imax supl Yo yo | ,可 取 325. 


图 16-8 多 智能 体系 统 结构 
根据 式 (16. 12) ,针对 图 16-8 中 的 四 个 智能 体 的 控制 律 设计 如 下 : 


ü; SS i +u: yo cy) DE psgnz; 
根据 图 16-8, 只 有 第 二 个 智能 体 与 yo 相连 , 则 m0 2 lu 0p, 二 0, 则 
Wi = = — Xe — msgnz, 


us = — f Ža — Tz 十 yn 十 yn ]:Sgnz,; 


Us =— C3 Z3 — X3) 一 N3 SENZ; 
ua 一 一 C4Z4 =Z T Jı SENZ, 
考虑 Dimas =d im F suplyo- yol Ht p 995.9, 3. WRR 16-8 和 式 (16.9), 有 
«ji e M5 


za = (ys y) ys — y); F ya = Ya? 
47 (ya — y2) + ya — ya) 
S4 7 M9 77s 
根据 式 (16.9) 可 得 z, =z; He 356 0, —20,1—1,2,3,4. A T Br iE BH P bl 38 3X (16. 12) 
中 ,采用 饱和 函数 satre 代替 符号 函数 sgnz ,设计 如 下 : 


ls TPA 
satz —4kr, |2|<K4, k=1/A 
—], 了 去 一 人 


其 中 ,A 为 边界 层 。 

Ht A —0. 003 ,运行 Simulink 仿真 主 程序 chap16_3sim. mdl, 仿 真 结 果 如 图 16-9 一 
图 16-11 所 示 。 

采用 M 语言 ,对 模型 进行 离散 化 ,也 可 以 实现 多 智能 体 控制 系统 的 仿真 ,仿真 程序 设计 
更 加 简单 方便 。 不足 之 处 是 计算 精度 较 差 。 取 离散 时 间 为 ==0. 001, 离 散 仿真 程序 为 
chapl6 3dis. m, 


ul 


u2 


u3 
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vmi 


ideal signal 
— — first agent 

second agent 
=== third agent 

fourth agent 


ideal signal speed 
f| — — first agent 
second agent 
== third agent 
— fourth agent 
V 


1.5 
0 2 4 6 8 10 


t 


E 16-10 多 智能 体系 统 的 速度 一 致 性 跟踪 


E LUE 
first agent 


0 
-00 2 4 6 8 10 
time 
20 
0 —— dá à 
-20 
0 2 4 6 8 10 
time 
5 
-5 
0 2 4 6 8 10 
time 
10 
-10 
0 2 4 6 8 10 
time 


图 16-11 多 智能 体系 统 的 控制 输入 
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仿真 程序 : 

1. 图 16-8 的 Laplacian 矩阵 分 析 仿 真 程序 chapl6 3L. m 
2. Simulink 仿真 程序 

(OD 主 程序 : chap16_3sim. mdl。 


chap16_3plant 


(2) 智能 体 控 制 器 子 程序 : chap16_3ctrl. m, 


function [sys, x0, str,ts] = func(t,x,u,flag) 
switch flag, 
case 0, 

[sys, x0, str, ts] = mdlInitializeSizes; 
case 1, 

sys = ndlDerivatives(t,x,u); 
case 3, 

sys = ndlOutputs(t,x,u); 
case (2,4,9) 

sys- []; 
otherwise 

error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0, str, ts] = mdlInitializeSizes 


sizes = simsizes; 


LU 


sizes. NumContStates 


sizes.NumDiscStates 
sizes.NumOutputs 


sizes.NumInputs 


1 


sizes. DirFeedthrough 


H 
m5 OO SO 
M 


LU 


sizes.NumSampleTimes 


sys = simsizes(sizes); 


x0 -[]; 
str = Dh 
ts = [00]; 


function sys = mdlOutputs(t, x, u) 
x11 =u(1);x12 = u(2); 
x21 = u(3);x22=u(4); 
x31 = u(5);x32 = u(6); 
x41 = u(7);x42 = u(8); 
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y0 = u(9); % sin(t); 
dy0 = cos(t); 
ddy0 = - sin(t); 


* First agent 
yl = x11;y2 = x21; y3 = x31; y4 = x41; 
dyl = x12;dy2 = x22;dy3 = x32;dy4 = x42; 


zl = yl- y2; 
dyl = x12; 

dz1 = dyl- dy2; 
z1lb = dz1 + z1; 
xitel = 5; 

c1 = 20; 


delta = 0. 003; 
kk = 1/delta; 
if abs(zlb)> delta 
sat zlb- sign(zlb); 
else 
sat zlb- kk * zlb; 
end 
utl- —cl*zlb-x12- xitel* sat zlh; 
% Second agent 
niu2 = 1; 
z2 = niu2 * (y2 - y0) + (y2 - y1) + (y2 - y3); 
dy2 = x22; dy3 = x32; 
dz2 = dy2 - dy0 + dy2 - dy1 + dy2 - dy3; 
z2b = dz2 + z2; 
xite2 = 3; 
c2 - 20; 


delta = 0.003; 
kk = 1/delta; 
if abs(z2b)» delta 
sat z2b- sign(z2b); 
else 
sat z2b- kk * z2b; 
end 
ut2 = - c2 * z2b- x22 + ddy0 + dy0 - xite2 * sat z2b; 
* Third agent 
z3- y3 — y2 * y3- y4; 
dz3 = dy3 - dy2 + dy3 - dy4; 
z3b = dz3 + z3; 
xite3 = 5; 
c3 = 20; 


delta = 0. 003; 

kk = 1/delta; 

if abs(z3b)> delta 
sat_z3b = sign(z3b); 
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else 
sat z3b- kk * z3b; 
end 
ut3 = — c3 * z3b- x32 - xite3 * sat z3b; 
* Fourth agent 
z4= y4- y3; 
dz4 7 dy4 - dy3; 
z4b = dz4 + z4; 
xite4 = 5; 
c4 = 20; 


delta = 0. 003; 
kk = 1/delta; 
if abs(z4b)> delta 
sat z4b- sign(z4b); 
else 
sat z4b- kk * z4b; 
end 
ut4 = — c4 * z4b - x42 - xite4 * sat z4b; 


sys(1) = utl; 
sys(2) = ut2; 
sys(3) = ut3; 
sys(4) = ut4; 


G) 智能 体 被 控 对 象 子 程序 : chapl6 3plant. m, 


function [sys, x0, str,ts] = Model(t, x, u, flag) 
switch flag, 
case 0, 

[sys, x0, str, ts] = ndlInitializeSizes; 
case 1, 

Sys = ndlDerivatives(t,x,u); 
case 3, 

sys = ndlOutputs(t,x,u); 
case (2, 4, 9 ) 


sys = []; 
otherwise 

error(['Unhandled flag = ',num2str(flag)]); 
end 


function [sys, x0, str,ts] = mdlInitializeSizes 
Sizes = simsizes; 

sizes.NumContStates - 
sizes.NumDiscStates = 


8 

0 
sizes.NumOutputs - 8; 
sizes.NumInputs -4 
sizes.DirFeedthrough = 0 
sizes. NumSampleTimes = 1 
sys = simsizes(sizes); 
x0-2[0.10.10.10.10.10.10.10.1]; 


str-[]; 
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ts-[-10]; 

function sys = ndlDerivatives(t, x, u) 
ut » [u(1) u(2) u(3) u(4)]; 

dt = [3 * sin(t) 3* sin(t) 3* sin(t) 3* sin(t)]; 
sys(1) = x(2); 
sys(2) » ut(1) + dt(1); 

sys(3) = x(4); 

sys(4) » ut(2) + dt(2); 

sys(5) = x(6); 

sys(6) = ut(3) + dt(3); 
sys(7) = x(8); 

sys(8) » ut(4) + dt(4); 


function sys = mdlOutputs(t, x, u) 
sys(1) = x(1); 
sys(2) = x(2); 
sys(3) = x(3); 
sys(4) = x(4); 
sys(5) * x(5); 
sys(6) = x(6); 
sys(7) - x(7); 
sys(8) = x(8); 


(4) 作 图 子 程序 : chapl6 3plot. m, 


close all; 

figure(1); 

plot(t, yO, 'k', t, y( :, 1), 'r', t, y( :, 3), 'b', t, y(:,5), 'g', t, y(:, 7), 'c', 'linewidth',2); 
xlabel('t');ylabel('y'); 

legend('ideal signal', 'first agent', 'second agent'', 'third agent', '£fourth agent'); 


figure(2); 
plot(t,cos(t), 'k',t, y(:,2) , 'r', t, y(:, 4), 'b', t, y(:,6), 'g', t, y(:,8), 'c', 'linewidth',2); 
legend('ideal signal speed', 'first agent', 'second agent', 'third agent', 'fourth agent'); 


figure(3); 

subplot(411); 

plot(t,ut(:,1), 'r', 'linewidth',2); 
xlabel('time');ylabel('ul'); 
legend('first agent'); 
subplot(412); 

plot(t,ut(:,2), 'r', 'linewidth',2); 
xlabel('time'); ylabel('u2'); 
legend( 'second agent'); 
subplot(413); 

plot(t,ut(:,3), 'r', 'linewidth',2); 
xlabel( 'time'); ylabel('u3'); 
legend('third agent'); 
subplot(414); 

plot(t,ut(:,4), 'r', 'linewidth',2); 
xlabel('time');ylabel('u4'); 
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legend( fourth agent'); 
2. M 语言 离散 仿真 程序 chapl6 3dis. m 


close all; 
close all; 


n = 4; %agent number 
z= [0.10.20.30.4]'; 
dx = [000 0]*; 

u = zeros(n,1); 


T = 0.001; 
t max - 10; 


n max = t max/T; 


miu = zeros(n,1); 
miu(2) = 1; 
miu = diag(miu); 


* Laplacian matrix 
L [1 -1 0 0; 
eu "E 0; 
0 42 2 
0 0 =I lb 


c = 30* eye(n); 
xite = 3 * eye(n); 


fori - 1:n max- 1 
y(i) = sin(T* i); 
dy(i)= cos(T * i); 
ddy(i) = - sin(T* i); 
di-0.3*sin(T* i) * ones(n,1); 


ei = x(:,end) - y(i); 
dei = dx(:,end) - dy(i); 
ei b = ei + dei; 

zi b = (L+ miu) * ei_b; 


% Saturated function 
delta = 0. 01; 
kk = 1/delta; 
fork=1:1:n 
if abs(zi b(k))» delta 
sati(k) » sign(zi b(k)); 
else 
sati(k) » kk * zi b(k); 
end 
end 
ui= —-c*zi b- dx(:,end) * miu * ones(n,1) * (dy(i) * ddy(i)) - xite * sati'; 
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* Plant 

ddxi = ui + di; 

dxi = dx( : , end) + ddxi * T; 
xi = x( :, end) + dxi * T; 


u= [uui]; 
dx = [dx dxi]; 
x= [x xi]; 
end 
T ze TTE max; 


figure(1); 

plot(T, sin(T), 'k', 'linewidth',2); 
hold on; 

plot(T,x(1, :), 'r', 'linewidth',2); 
hold on; 

plot(T,x(2, :), 'b', 'linewidth',2); 
hold on; 

plot(T, x(3, :), 'g', 'linewidth',2); 
hold on; 

plot(T,x(4, :), 'm', 'linewidth',2); 
legend( 'ideal', 'first', 'second', 'third', 'fourth'); 
ylabel('x tracking'); 


figure(2) 

plot(T,cos(T), 'k', 'linewidth',2); 
hold on; 

plot(T, dx(1, :), 'r', 'linewidth',2); 
hold on; 

plot(T,dx(2, :), 'b', 'linewidth',2); 
hold on; 

plot(T,dx(3,:), 'g', 'linewidth',2); 
hold on; 

plot(T, dx(4, :), 'n', 'linewidth',2); 
legend( 'ideal', 'first', 'second', 'third', 'fourth'); 
ylabel('dx tracking'); 


figure(3); 

plot(T,u(1,:), 'r', 'linewidth',2); 

hold on; 

plot(T,u(2, :), 'b', 'linewidth',2); 

hold on; 

plot(T,u(3, :), 'g', 'linewidth',2); 

hold on; 

plot(T, u(4, :), 'n', 'linewidth',2); 

legend( 'first', 'second', 'third', 'fourth'); 
ylabel('control input'); 
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附录 


Laplacian 4 EF Zr Jf 
根据 图 16-8, 可 得 


010 0 100 0 0 0 0 0 
mo 1010 g. 0200 0100 
i dy EO 1| ^ oe20|' ','CÓ looo o 
00 10 000 1 0000 
则 
1000 010 0 iÓ^-1 6 (1 
0200 I16061209| l-i1 $ e hb 
L-H—A- - = 
0 0 2 0 0 10 1 =i a] 
000 1 00 10 -wA 1 
ùt $6 8$ 0 0 0 0 | = 
=] =] 0100 l1 5 =i 6 
Li+i+x= 十 一 
(| 0 0 0 0 Bol d ej 
0 0 —1 1 0000 0 © = ] 


特征 值 为 : eig(L -- p) — [0. 1729,0. 6617,2. 2091,3. 9563], 9l L+ 为 正定 阵 。 根 据 
A(16.10),2,—0 HM ,6,—0, xL ES 8942-39 chapló 3L. m, 


16.5 基于 RBF 网 络 的 多 智能 体系 统一 致 性 控制 


16.5.1 系统 描述 
针对 如 下 二 阶 线性 多 智能 体系 统 : 
Za =u; Hfi) +d; 
yi 三 Tn (16.13) 


其 中 ,|d; | 三 4d,,,, Guo EE 表示 智能 体 i 可 以 获得 智能 体 ; Bd f Gra ,xis) 为 连续 可 
导 的 未 知 函 数 。 智 能 体 ; 的 相 邻 集合 表示 为 A;=={j1(j ,i)EE)。 

智能 体 i 与 智能 体 j 之 间 连 接 的 标记 取 a, ,ay —1 时 表示 智能 体 ; 与 智能 体 ) 之 间 有 
通信 ,否则 a; —0. HB a;—0.A-—[a; JER, 

4E XB —diag(E;*-,Ey). E; — Sas , 定义 Laplacian 矩阵 为 


j71 


L—H-—A (16. 14) 
1678 y, ,智能 体 i 与 指令 yo 之 间 连 接 的 标记 取 pu; 1 时 表示 智能 体 ; 可 以 获得 
指令 y, 信息 ,否则 取 /一 0, 取 
p — diaglp; »* ix) 
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控制 日 标 为 s 1 一 co 时 a Yo nT o 


16.5.2 基于 RBF Dg fC- ) 的 滑 模 控制 
采用 RBF 神经 网 络 通 近 f(。),RBF 网 络 算法 为 
h; =exp( | AT | -| 


2b; 
f —-W''hox)4-8 
其 中 ,x 为 网 络 的 输入 ,i 为 网 络 的 输入 个 数 ,i 为 网 络 隐 含 层 第 j 个 节点 ,h 王 [h;] 为 高 斯 


函数 的 输出 ,W" 为 网 络 的 理想 权 值 ,6 为 网 络 的 允 近 误差 ,日 1616、。 
网 络 的 输入 取 x, 则 RBF 网 络 的 输出 为 


Fla) Wh) (16. 15) 
其 中 ,h(x) 为 RBF #2 I 28 85 55 3r RR 


16.5.3 控制 律 的 设计 
智能 体 i 的 跟踪 误差 为 e, — y, v ,定义 


E; —€, FE 

zi =u: (y — y) + À (i — y») (16. 16) 
JEAi 

z,—z,-z, 


由 于 y,—y, 7e, —e; ,根据 ay 的 定义 ,有 
z; 一 NiEi 十 PNG —£,,) = Cp; FA — ase, 
p p 


由 于 Jape, —Ae, Jil 


z —(u--H)€—A€ —(L--u)€ (16.17) 
根据 控制 目标 ,设计 Lyapunov 函数 为 
N 
V=7 EL Fg E+ 37 WW (16. 18) 
i=l 
其 中 ,7 二 0。 
则 
N "m PA N - A 
V—e€"(L-g)€- Y X WIW, =z" €—- y X) WIW, 
i=l i=l 
由 于 


È; —6, HE; =y; 一 yo HY; 一 yo 
一 由 十 jznyza) 十 di 十 za 一 后 (yo 十 yo) 十 (一 1)(0yo 十 yo) 
=ü; Hf tasta) t+ xia Hio Hyd + D, 
其 中 ,D;=d; 十 (pj 一 1) (yo 十 y0) sD imax =d ima supl yo yol« 


430 «(|| 机 器 人 控制 系统 的 设计 与 MATLAB 仿 真 ， 基本 设计 方法 (第 2 版 ) 


E X x, [xa]. M 


N N . 
V — Mz, Q, - f x0 xa — ni Go E) DO —yY 39WIW, 
i=l i=l 


设计 控制 律 为 
ü =À iz; F; )— ziz uua 十 >o) 一 1i SENZ; (16. 19) 
其 中 sN: >D imax FÔN sÀ; >00. 
由 于 


fa) —fQGj) 9W/"h(,)--8;  WIh(x)) —WhG,) 4- 8, 


其 中 ,W, =w; 一 W,。 


则 
y N u N EM 
V= J z C A;z, — nsgnz 4WIh(x,) +8; - DO — Y WW, 
i=] i-1 
N 35 A 
= J z; C Az, — g,sgnz, +8; 4- Dj) + WI Gh Gc) — YW,) 
i=} 
设计 自 适 应 律 为 


W, -EhGO (16. 20) 


N 
V=- ez <0 


i 


=] 
M 41 一 0 时 ,zx, 一 0, 根 据 式 (16. 17) ,e, 一 0, 从 而 e, 一 0 He;—0, 


16.5.4 仿真 实例 


考虑 如 图 16-8 所 示 的 多 智能 体系 统 结构 "" ,只 有 第 二 个 智能 体 与 指令 y, 相连 ,ys 二 1， 
yo 三 sint。 针 对 多 智能 体系 统 式 (16. 13), fA Cra oma) — rario d,—3sint, i —1,2,3,4,?H 
i—2 Bl Dium dius BU 23; M 1—1,3,4 BE D ima =d im supl Ya yol » BIB 925. 

根据 式 (16. 190 ,针对 图 16-8 中 的 四 个 智能 体 的 控制 律 设计 如 下 : 


ü; ——2À£, —f x) — xa + p, Gro F4) — qsgnz, 
根据 图 16-8, 只 有 第 二 个 智能 体 与 yo 相连 , 则 Ai =0, u: =l, u; =0, y, =0. M] 
ui 三 :一 类 TS =f X ) 一 和 一 71Sgnx1 


üg =— Mz —f(x1) — Esz 十 yo Fia ]:sgnz; 


us 一 一 人 3Y3 fj X32 N3 SENZ; 
Us =— Àz, fæ) fa, — M SENZ, 
考虑 D imax =d ims F SUp| Yo H Yo | , 取 UI —5717,795,9»—3., 根据 图 16-8 FRAG. 9) ,有 
za = Yi ~ yz 


zZ = (y — yo) + (ys = y1) (ys — y3) 


人 


za = (ya — y2) + (Ys — ya? 
Za —y47- Ys 
根据 式 (16. 160 ,可 得 z, — 2, Hz; 0A, —20.1—1.2,3.4, X T Br ab BH d dll a Po 
用 饱和 函数 sate 代替 符号 函数 sgna ,设计 如 下 : 


ls e xegÀ 
satr —4br, |x|&A,. hb-—l/A 
— a m ZA 


其 中 ,A 为 边界 层 。 仿 真 中 取 A 王 0. 003。 

神经 网 络 的 结构 取 为 2 一 5 一 1,c, Ho, 分 别 设置 为 [一 1.0 一 0.5 0 0.5 1.0] 和 
b; 二 0.20, 网 络 的 初始 权 值 为 0. 0。 

采用 控制 律 式 (16. 19) 和 自 适应 律 式 (16. 20), 自 适应 参数 取 y= 二 0.15。 仿真 结 果 如 
图 16-12 一 图 16-14 所 示 。 


1.5 r : r ———— —À 
ideal signal 


— first agent 
second agent 
a third agent 

fourth agent 


2 4 
图 16-12 多 智能 体系 统 的 位 置 一 致 性 跟踪 


= ideal signal speed 
| —— first agent 
second agent 
third agent 
~~ fourth agent 


16-13 多 智能 体系 统 的 速度 一 致 性 跟踪 
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| 
* 0 first agent 


p NENNEN EE ám. 
g of ~= second agent 


2 4 6 8 10 
time 
5 
$e en] 
-5 
0 2 4 6 8 10 


fourth agent 


$ 中 一 一 一 一 一 一 一 一 [一 一 wee 
-10 
0 


Simulink 仿真 程序 : 
(OD) 主 程序 . chap16_4sim. mdl, 


time 


16-14 多 智能 体系 统 的 控制 输入 


(2) 智能 体 控 制 器 子 程序 : chapl6 4ctrl. m, 


function [sys,xO,str,ts] = func(t,x,u,flag) 
switch flag, 
case 0, 

[sys, x0, str, ts] = mdlInitializeSizes; 
case 1, 

sys = ndlDerivatives(t,x,u); 
case 3, 

sys = ndlOutputs(t, x, u); 
case {2,4,9} 

sys=[]; 
otherwise 

error(['Unhandled flag = ',num2str(flag)]); 
end 
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function [sys, x0, str, ts] = ndlInitializeSizes 
global cij bj 

sizes = simsizes; 
sizes.NumContStates 


ll 
N 
e 
~ 


sizes. NumDiscStates 


sizes.NumOutputs 
sizes.NumInputs 
sizes.DirFeedthrough 


sizes. NumSampleTimes 


ii 
e e O A o 
` 


sys = simsizes(sizes); 
x0 = 0x ones(1,20); 
str = []; 
ts = [00]; 
cije[-1 —0.50 0.51; 
—1 0.50 0.51]; 
bj = 0.20; 
function sys = mdlDerivatives(t, x, u) 
global cij bj 
x11 = u(1);x12 = u(2); 
x21 = u(3);x22 = u(4); 
x31 = u(5);x32 = u(6); 
x41 = u(7);x42 = u(8); 
yl = x11;y2 = x21; y3 = x31; y4 = x41; 
dy1 = x12; dy2 = x22; dy3 = x32; dy4 = x42; 


y0 =u(9); % sin(t); 
dy0 = cos(t); 
ddy0 = - sin(t); 


% First agent 
zl = yl - y2; 
dyl = x12; 

dzl = dyl - dy2; 
zlb = dzl + z1; 


xil = [x11;x12]; 
hl = zeros(5,1); 
for j=1:1:5 
hi(j) = exp( - norm(xil - cij(:,j))^2/(2 * bj^2)); 
end 


% Second agent 

niu2 = 1; 

z2 = niu2 * (y2 - y0) + (y2 - y1) + (y2 - y3); 
dy2 = x22; dy3 = x32; 

dz2 = dy2 - dy0 + dy2 - dyl + dy2 - dy3; 

z2b = dz2 + z2; 


xi2 = [x21;x22]; 
h2 = zeros(5,1); 
for j=1:1:5 
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h2(j) = exp( - norn(xi2- cij(:,3))^2/(2 * bj^2)); 
end 


* Third agent 
z3-7y3- y2 + y3- y4; 

dz3 = dy3 - dy2 + dy3 - dy4; 
z3b = dz3 + z3; 


xig = [x31;x32]; 
h3 = zeros(5, 1); 
for J= 1:1:5 
h3(j) = exp( - norm(xi3 - cij(:,3j))^2/(2 * bj^2)); 
end 


* Fourth agent 
z4- y4- y3; 
dz4 = dy4 - dy3; 
z4b = dz4 + z4; 


xi4 = [x41;x42]; 
h4 = zeros(5,1); 
for j=1:1:5 
h4(j) = exp( - norn(xi4 - cij(:,3))^2/(2 * bj^2)); 


end 
gama = 0.15; 
for i-21:1:5 


sys(i)- - l/gama * zlb* hl(i); 

sys(i+5)= - 1/gama * z2b * h2(i); 

sys(i*10)- - 1/gama * z3b * h3(i); 

sys(i*15)- - 1/gama * z4b * h4( i); 
end 


function sys = ndlOutputs(t, x, u) 
global cij bj 

xll-2u(1);x12-7 u(2); 

x21 = u(3);x22-7 u(4); 

x31 = u(5);x32 = u(6); 

x41 = u(7);x42 = u(8); 

yl = x11;y2 = x21; y3 = x31; y4 = x41; 

dyl = x12; dy2 = x22;dy3 = x32; dy4 = x42; 


y0 7 u(9); * sin(t); 
dy0 = cos(t); 
ddy0 = - sin(t); 


* First agent 
zl- yl- y2; 
dyl = x12; 

dzl = dyl - dy2; 
zlb = dz1 + z1; 
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xitel = 5; 
Lambdal = 20; 


W1 = [x(1) x(2) x(3) x(4) x(5)]'; 
xil = [x1l1;x12]; 
hl = zeros(5,1); 
forj-21:1:5 
h1(j) = exp( - norn(xil — cij(:,3))^2/(2 * bj^2)); 
end 
fln-W1'*hl; 


delta = 0.003; 
kk = 1/delta; 
if abs(zlb)» delta 
sat zlb- sign(zlb); 
else 
sat zlb- kk * zlb; 
end 
ul = - Lambdal * zlb- fin- x12- xitel * sat zlb; 


% Second agent 
niu2= 1; 
z2 = niu2 * (y2 - y0) + (y2- y1) + (y2 - y3); 
dy2 = x22; dy3 = x32; 
dz2 = dy2 - dy0 + dy2 - dy1 + dy2 — dy3; 
z2b = dz2 + z2; 
xite2 = 3; 
Lambda2 = 20; 
W2 = [x(6) x(7) x(8) x(9) x(10)]'; 
xi2-[x21;x22]; 
h2 = zeros(5,1); 
for j721:1:5 
h2(3) = exp( - norn(xi2 - cij(:,3))^2/(2 * bj^2)); 
end 
f2n = W2' * h2; 


delta = 0.003; 
kk = 1/delta; 
if abs(z2b)» delta 
sat z2b- sign(z2b); 
else 
sat z2b- kk * z2b; 
end 
u2 = — Lambda2 * z2b - f2n - x22 + ddy0 + dy0 - xite2 * sat z2b; 
% Third agent 
z3 = y3 - y2 + y3- y4; 
dz3 = dy3 - dy2 + dy3 - dy4; 
z3b = dz3 + z3; 
xite3 = 5; 
Lambda3 = 20; 


436 «(|| 机 器 人 控制 系统 的 设计 与 MATLAB 仿 真 ， 基 本 设计 方法 (第 2 版 ) 


W3= [x(11) x(12) x(13) x(14) x(15)]'; 
xi3 = [x31;x32]; 
h3 = zeros(5,1); 
forj-21:1:5 
h3(j) = exp( - norn(xi3 - cij(:,j3))^2/(2 * bj^2)); 
end 
f3n = W3' * h3; 


delta = 0. 003; 
kk = 1/delta; 
if abs(z3b)> delta 
sat z3b- sign(z3b); 
else 
sat z3b- kk * z3b; 
end 
u3 = - Lambda3 * z3b - f3n - x32 - xite3 * sat z3b; 


* Fourth agent 
z4= y4- y3; 
dz4 = dy4 - dy3; 
z4b = dz4 + z4; 
xite4 = 5; 
Lambda4 - 20; 


W4 = [x(16) x(17) x(18) x(19) x(20)]'; 
xi4 = [x41;x42]; 
h4 = zeros(5,1); 
for j21:1:5 
h4(3) = exp( - norn(xi4 - cij(:,3j))^2/(2 * bj^2)); 
end 
f4n- W4' x h4; 


delta = 0.003; 
kk = 1/delta; 
if abs(z4b)» delta 
sat z4b- sign(z4b); 
else 
sat z4b- kk * z4b; 
end 
u4 = — Lambda4 * z4b — f4n - x42 - xite4 * sat z4b; 


sys(1)7ul; 
sys(2) = u2; 
sys(3) = u3; 
sys(4) = u4; 


(3) 智能 体 被 控 对 象 子 程序 : chapl6_4plant. m, 


function [sys, x0, str, ts] = Model(t, x,u, flag) 
switch flag, 
case 0, 
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[sys, x0, str, ts] = ndlInitializeSizes; 
case 1l, 

sys = ndlDerivatives(t,x,u); 
case 3, 

sys = ndlOutputs(t, x, u); 
case (2, 4, 9 ) 

sys = []; 
otherwise 

error(['Unhandled flag = ',num2str(flag)]); 
end 
function [sys, x0, str,ts] = ndlInitializeSizes 


sizes - simsizes; 


sizes.NumContStates 


sizes.NumDiscStates 


sizes.NumOutputs 


sizes.NumInputs 


sizes.DirFeedthrough 


Li 
e O A oo oo 


sizes. NumSampleTimes 
sys = simsizes(sizes); 
x0-[0.10.10.10.10.10.10.10.1]; 
str-[]; 

ts-[-10]; 

function sys = ndlDerivatives(t,x,u) 
utl = u(1); 

fxl-7x(1)*x(2); 

dl=3x sin(t); 

ut2 = u(2); 

fx2 = x(1) * x(2); 

d2-23* sin(t); 

ut3 = u(3); 

fx3- x(1) * x(2); 

d3-23* sin(t); 

ut4 = u(4); 

fx4 = x(1) * x(2); 

d4 = 3 * sin(t); 

sys(1) = x(2); 

sys(2) = utl + fx1 + d1; 
sys(3) = x(4); 

sys(4) = ut2 + fx3 + d2; 

sys(5) = x(6); 

sys(6) = ut3 + fx3 + d3; 

sys(7) = x(8); 

Sys(8) = ut4 + fx4 + d4; 

function sys = mdlOutputs(t, x, u) 
sys(1) = x(1); 

sys(2) = x(2); 

sys(3) = x(3); 

sys(4) = x(4); 
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[1] 
[2] 
[3] 
[4] 
[5] 


[6) 


sys(5) = x(5); 
sys(6) = x(6); 
sys(7) = x(7); 
sys(8) = x(8); 


(4) 作 图 子 程序 : chapl6 4plot. m, 


close all; 

figure(1); 

plot(t,y0, 'k', t, g( Th 'z^, t, y( :,3),, 'b', t, y( :, 5), "g', t, y( 3,7), "ct; 1 inewidth';2); 
xlabel('t');ylabel('y'); 


legend('ideal signal', 'first agent', 'second agent', 'third agent', 'fourth agent'); 


figure(2); 

plot(t,cos(t),'k',t,v( 2,2), 'r', t, v( 1,4), D t, v( 2,6), 'g', t, v( :, 8). 'c*, *'ÀinewidEh', 2); 
xlabel('t');ylabel('y'); 

legend('ideal signal speed', 'first agent', 'second agent', 'third agent', 'fourth agent'); 


figure(3); 

subplot(411); 

plot(t,ut(:,1), 'r', 'linewidth',2); 
xlabel('time'); ylabel('ul'); 
legend('first agent'); 
subplot(412); 

plot(t,ut(:,2), 'r', 'linewidth',2); 
xlabel('time'); ylabel('u2'); 
legend( 'second agent') ; 
subplot(413); 

plot(t,ut(:,3), 'r', 'linewidth',2); 
xlabel( 'time');ylabel('u3'); 
legend( 'third agent'); 
subplot(414); 

plot(t,ut(:,4), 'r', 'linewidth',2); 
xlabel('time'); ylabel('u4'); 
legend( 'fourth agent'); 
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本 书 主要 以 机 械 手 为 被 控 对 象 ， 系 统 地 论述 了 机 器 人 控制 系统 设计 的 基本 设计 方法 ， 包 括 先进 PD 控 

制 、 神 经 网 络 自 适 应 控制 、 模 糊 自 适 应 控制 、 迭 代 学 习 控 制 、 反 演 控制 、 滑 模 控 制 、 自 适应 鲁 棱 控制 、 

末端 轨迹 及 力 的 连续 切换 滑 膜 控制 、 重 复 控 制 、 传 感 器 和 执行 器 容错 控制 、 事 件 驱动 控制 、 输 入 延迟 控 

制 、 执 行 器 量化 控制 、 控 制 方向 未 知 的 控制 和 多 智能 体系 统一 致 性 控制 的 设计 与 分 析 。 每 种 方法 都 给 出 

了 算法 推导 、 实 例 分 析 和 相应 的 MATLAB 设 计 仿 真 程 序 。 本 书 特点 如 下 : 

e ”机 器 人 控制 系统 的 控制 算法 重点 在 于 基础 理论 分 析 ， 针 对 机 械 手 基本 控制 算法 进行 了 深入 剖析 。 

€ ”所 有 控制 算法 均 给 出 了 完整 的 MATLAB 仿 真 程序 ， 也 给 出 了 程序 的 说 明和 仿真 结果 ， 有 具有 很 强 的 
可 读 性 。 

€ ”从 应 用 的 角度 出 发 ， 理 论 联系 实际 ， 面 向 广大 科研 与 工程 技术 人 员 ， 具 有 很 高 的 工程 实用 价值 。 

e ”控制 算法 及 应 用 实例 的 程序 结构 设计 简单 明了 ， 便 于 读者 学 习 和 二 次 开发 。 
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